diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml
index 4fcf2661ad20e..b882a4c1da42e 100644
--- a/planning/mission_planner/package.xml
+++ b/planning/mission_planner/package.xml
@@ -9,6 +9,7 @@
Ryohsuke Mitsudome
Takamasa Horibe
Takayuki Murooka
+ Mamoru Sobue
Apache License 2.0
Ryohsuke Mitsudome
diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
index e98084204ba78..c70a0e5d0176b 100644
--- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
+++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
@@ -195,7 +195,6 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
{
lanelet::ConstLanelets route_lanelets;
lanelet::ConstLanelets end_lanelets;
- lanelet::ConstLanelets normal_lanelets;
lanelet::ConstLanelets goal_lanelets;
for (const auto & route_section : route.segments) {
@@ -210,16 +209,14 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
}
}
- std_msgs::msg::ColorRGBA cl_route;
- std_msgs::msg::ColorRGBA cl_ll_borders;
- std_msgs::msg::ColorRGBA cl_end;
- std_msgs::msg::ColorRGBA cl_normal;
- std_msgs::msg::ColorRGBA cl_goal;
- set_color(&cl_route, 0.8, 0.99, 0.8, 0.15);
- set_color(&cl_goal, 0.2, 0.4, 0.4, 0.05);
- set_color(&cl_end, 0.2, 0.2, 0.4, 0.05);
- set_color(&cl_normal, 0.2, 0.4, 0.2, 0.05);
- set_color(&cl_ll_borders, 1.0, 1.0, 1.0, 0.999);
+ const std_msgs::msg::ColorRGBA cl_route =
+ tier4_autoware_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15);
+ const std_msgs::msg::ColorRGBA cl_ll_borders =
+ tier4_autoware_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05);
+ const std_msgs::msg::ColorRGBA cl_end =
+ tier4_autoware_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05);
+ const std_msgs::msg::ColorRGBA cl_goal =
+ tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999);
visualization_msgs::msg::MarkerArray route_marker_array;
insert_marker_array(
@@ -231,9 +228,6 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
insert_marker_array(
&route_marker_array,
lanelet::visualization::laneletsAsTriangleMarkerArray("end_lanelets", end_lanelets, cl_end));
- insert_marker_array(
- &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray(
- "normal_lanelets", normal_lanelets, cl_normal));
insert_marker_array(
&route_marker_array,
lanelet::visualization::laneletsAsTriangleMarkerArray("goal_lanelets", goal_lanelets, cl_goal));
@@ -270,23 +264,18 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
return msg;
}
-bool DefaultPlanner::check_goal_footprint(
+bool DefaultPlanner::check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet,
const lanelet::ConstLanelet & combined_prev_lanelet,
const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length,
const double search_margin)
{
- std::vector points_intersection;
-
// check if goal footprint is in current lane
- boost::geometry::intersection(
- goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon(), points_intersection);
- if (points_intersection.empty()) {
+ if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) {
return true;
}
- points_intersection.clear();
- // check if goal footprint is in between many lanelets
+ // check if goal footprint is in between many lanelets in depth-first search manner
for (const auto & next_lane : routing_graph_ptr_->following(current_lanelet)) {
next_lane_length += lanelet::utils::getLaneletLength2d(next_lane);
lanelet::ConstLanelets lanelets;
@@ -295,17 +284,20 @@ bool DefaultPlanner::check_goal_footprint(
lanelet::ConstLanelet combined_lanelets =
combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_);
- // if next lanelet length longer than vehicle longitudinal offset
+ // if next lanelet length is longer than vehicle longitudinal offset
if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
- boost::geometry::intersection(
- goal_footprint, combined_lanelets.polygon2d().basicPolygon(), points_intersection);
- if (points_intersection.empty()) {
+ // and if the goal_footprint is within the (accumulated) combined_lanelets, terminate the
+ // query
+ if (boost::geometry::within(goal_footprint, combined_lanelets.polygon2d().basicPolygon())) {
return true;
}
- points_intersection.clear();
- } else { // if next lanelet length shorter than vehicle longitudinal offset -> recursive call
- if (!check_goal_footprint(next_lane, combined_lanelets, goal_footprint, next_lane_length)) {
+ // if not, iteration continues to next next_lane, and this subtree is terminated
+ } else { // if next lanelet length is shorter than vehicle longitudinal offset, check the
+ // overlap with the polygon including the next_lane(s) until the additional lanes get
+ // longer than ego vehicle length
+ if (!check_goal_footprint_inside_lanes(
+ next_lane, combined_lanelets, goal_footprint, next_lane_length)) {
next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
continue;
} else {
@@ -352,13 +344,13 @@ bool DefaultPlanner::is_goal_valid(
double next_lane_length = 0.0;
// combine calculated route lanelets
- lanelet::ConstLanelet combined_prev_lanelet =
+ const lanelet::ConstLanelet combined_prev_lanelet =
combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_);
// check if goal footprint exceeds lane when the goal isn't in parking_lot
if (
param_.check_footprint_inside_lanes &&
- !check_goal_footprint(
+ !check_goal_footprint_inside_lanes(
closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
!is_in_parking_lot(
lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_),
diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
index 251d0db533182..9f1a9d206e376 100644
--- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
+++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
@@ -71,13 +71,38 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
void initialize_common(rclcpp::Node * node);
void map_callback(const HADMapBin::ConstSharedPtr msg);
- bool check_goal_footprint(
+
+ /**
+ * @brief check if the goal_footprint is within the combined lanelet of route_lanelets plus the
+ * succeeding lanelets around the goal
+ * @attention this function will terminate when the accumulated search length from the initial
+ * current_lanelet exceeds max_longitudinal_offset_m + search_margin, so under normal assumptions
+ * (i.e. the map is composed of finite elements of practically normal sized lanelets), it is
+ * assured to terminate
+ * @param current_lanelet the start lanelet to begin recursive query
+ * @param combined_prev_lanelet initial entire route_lanelets plus the small consecutive lanelets
+ * around the goal during the query
+ * @param next_lane_length the accumulated total length from the start lanelet of the search to
+ * the lanelet of current goal query
+ */
+ bool check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet,
const lanelet::ConstLanelet & combined_prev_lanelet,
const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length,
const double search_margin = 2.0);
+
+ /**
+ * @brief return true if (1)the goal is in parking area or (2)the goal is on the lanes and the
+ * footprint around the goal does not overlap the lanes
+ */
bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets);
+
+ /**
+ * @brief project the specified goal pose onto the goal lanelet(the last preferred lanelet of
+ * route_sections) and return the z-aligned goal position
+ */
Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections);
+
void updateRoute(const PlannerPlugin::LaneletRoute & route);
void clearRoute();
};
diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp
index e3983b7f3b962..4ef4bd6f20ed0 100644
--- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp
+++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp
@@ -21,11 +21,6 @@
#include
#include
-bool exists(const std::unordered_set & set, const lanelet::Id & id)
-{
- return set.find(id) != set.end();
-}
-
tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon(
tier4_autoware_utils::LinearRing2d footprint)
{
@@ -40,14 +35,6 @@ tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon(
return footprint_polygon;
}
-void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a)
-{
- cl->r = r;
- cl->g = g;
- cl->b = b;
- cl->a = a;
-}
-
void insert_marker_array(
visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2)
{
@@ -64,7 +51,7 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder(
std::vector right_bound_ids;
for (const auto & llt : lanelets) {
- if (llt.id() != 0) {
+ if (llt.id() != lanelet::InvalId) {
left_bound_ids.push_back(llt.leftBound().id());
right_bound_ids.push_back(llt.rightBound().id());
}
@@ -96,6 +83,8 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder(
add_bound(left_shared_shoulder_it->leftBound(), lefts);
} else if (
// if not exist, add left bound of lanelet to lefts
+ // if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`,
+ // then its left bound constitutes the left boundary of the entire merged lanelet
std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
add_bound(llt.leftBound(), lefts);
}
@@ -108,6 +97,8 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder(
add_bound(right_shared_shoulder_it->rightBound(), rights);
} else if (
// if not exist, add right bound of lanelet to rights
+ // if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`,
+ // then its right bound constitutes the right boundary of the entire merged lanelet
std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
add_bound(llt.rightBound(), rights);
}
diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp
index 3287267a00dfe..3eb38638e17b7 100644
--- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp
+++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp
@@ -30,8 +30,6 @@
#include
#include
-bool exists(const std::unordered_set & set, const lanelet::Id & id);
-
template
bool exists(const std::vector & vectors, const T & item)
{
@@ -45,12 +43,18 @@ bool exists(const std::vector & vectors, const T & item)
tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon(
tier4_autoware_utils::LinearRing2d footprint);
-void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a);
void insert_marker_array(
visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2);
+/**
+ * @brief create a single merged lanelet whose left/right bounds consist of the leftmost/rightmost
+ * bound of the original lanelets or the left/right bound of its adjacent road_shoulder respectively
+ * @param lanelets topologically sorted sequence of lanelets
+ * @param shoulder_lanelets list of possible road_shoulder lanelets to combine with lanelets
+ */
lanelet::ConstLanelet combine_lanelets_with_shoulder(
const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets);
+
std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet);
geometry_msgs::msg::Pose convertBasicPoint3dToPose(
const lanelet::BasicPoint3d & point, const double lane_yaw);