Skip to content

Commit 81a84ca

Browse files
authored
Merge branch 'main' into chore/pointpainting_launcher
2 parents 19e0e5b + dd52489 commit 81a84ca

File tree

6 files changed

+68
-58
lines changed

6 files changed

+68
-58
lines changed

.github/CODEOWNERS

+6-9
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,6 @@ common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp
4242
common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp
4343
common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp
4444
common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp
45-
common/tier4_debug_tools/** ryohsuke.mitsudome@tier4.jp
4645
common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp
4746
common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp
4847
common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp
@@ -124,11 +123,11 @@ perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
124123
perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp
125124
perception/euclidean_cluster/** yukihiro.saito@tier4.jp
126125
perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
127-
perception/image_projection_based_fusion/** dai.nguyen@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
126+
perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
128127
perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp
129128
perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp
130129
perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp
131-
perception/lidar_centerpoint/** kenzo.lobos@tier4.jp
130+
perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp
132131
perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp
133132
perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp
134133
perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
@@ -192,10 +191,9 @@ planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@ti
192191
planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp
193192
planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
194193
planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp
195-
planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp
194+
planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp
196195
planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp
197196
planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp
198-
planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
199197
planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp
200198
planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp
201199
planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp
@@ -209,12 +207,12 @@ planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp
209207
planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
210208
planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
211209
planning/surround_obstacle_checker/** satoshi.ota@tier4.jp
212-
sensing/gnss_poser/** koji.minoda@tier4.jp yamato.ando@tier4.jp
210+
sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
213211
sensing/image_diagnostics/** dai.nguyen@tier4.jp
214212
sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp
215213
sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
216214
sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp
217-
sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
215+
sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
218216
sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
219217
sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
220218
sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
@@ -242,8 +240,7 @@ system/system_error_monitor/** fumihito.ito@tier4.jp
242240
system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp
243241
system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp
244242
system/velodyne_monitor/** fumihito.ito@tier4.jp
245-
tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
246-
vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp
243+
vehicle/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp
247244
vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp
248245
vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp
249246
vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp

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)