From 7a70a3dd0798c25cf2c805a5b6e603f256baa64c Mon Sep 17 00:00:00 2001 From: h-ohta Date: Wed, 18 May 2022 18:21:43 +0900 Subject: [PATCH 1/3] feat: visualize lane boundaries --- map/lanelet2_extension/lib/visualization.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/map/lanelet2_extension/lib/visualization.cpp b/map/lanelet2_extension/lib/visualization.cpp index fc9f068669d96..d464f20ff98e5 100644 --- a/map/lanelet2_extension/lib/visualization.cpp +++ b/map/lanelet2_extension/lib/visualization.cpp @@ -878,11 +878,13 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra double lss_center = std::max(lss * 0.1, 0.02); std::unordered_set added; - visualization_msgs::msg::Marker left_line_strip, right_line_strip, center_line_strip; + visualization_msgs::msg::Marker left_line_strip, right_line_strip, lane_bound_line_strip, center_line_strip; visualization::initLineStringMarker( &left_line_strip, "map", additional_namespace + "left_lane_bound", c); visualization::initLineStringMarker( &right_line_strip, "map", additional_namespace + "right_lane_bound", c); + visualization::initLineStringMarker( + &lane_bound_line_strip, "map", additional_namespace + "lane_bound", c); visualization::initLineStringMarker( ¢er_line_strip, "map", additional_namespace + "center_lane_line", c); @@ -892,6 +894,11 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra lanelet::ConstLineString3d left_ls = lll.leftBound(); lanelet::ConstLineString3d right_ls = lll.rightBound(); lanelet::ConstLineString3d center_ls = lll.centerline(); + lanelet::LineString3d lane_bound_ls(lanelet::utils::getId()); + lane_bound_ls.push_back(lanelet::Point3d( + lanelet::utils::getId(), left_ls.front().x(), left_ls.front().y(), left_ls.front().z())); + lane_bound_ls.push_back(lanelet::Point3d( + lanelet::utils::getId(), right_ls.front().x(), right_ls.front().y(), right_ls.front().z())); if (!exists(added, left_ls.id())) { visualization::pushLineStringMarker(&left_line_strip, left_ls, c, lss); @@ -901,6 +908,10 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra visualization::pushLineStringMarker(&right_line_strip, right_ls, c, lss); added.insert(right_ls.id()); } + if (!exists(added, lane_bound_ls.id())) { + visualization::pushLineStringMarker(&lane_bound_line_strip, lane_bound_ls, c, lss); + added.insert(lane_bound_ls.id()); + } if (viz_centerline && !exists(added, center_ls.id())) { visualization::pushLineStringMarker(¢er_line_strip, center_ls, c, lss_center); added.insert(center_ls.id()); @@ -917,6 +928,9 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra if (!center_line_strip.points.empty()) { marker_array.markers.push_back(center_line_strip); } + if (!lane_bound_line_strip.points.empty()) { + marker_array.markers.push_back(lane_bound_line_strip); + } return marker_array; } From bab2021b97096010007d8ac804c2a7af9848d437 Mon Sep 17 00:00:00 2001 From: h-ohta Date: Wed, 18 May 2022 21:17:47 +0900 Subject: [PATCH 2/3] fix: start_bound --- map/lanelet2_extension/lib/visualization.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/map/lanelet2_extension/lib/visualization.cpp b/map/lanelet2_extension/lib/visualization.cpp index d464f20ff98e5..441ab4c93e14f 100644 --- a/map/lanelet2_extension/lib/visualization.cpp +++ b/map/lanelet2_extension/lib/visualization.cpp @@ -878,13 +878,13 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra double lss_center = std::max(lss * 0.1, 0.02); std::unordered_set added; - visualization_msgs::msg::Marker left_line_strip, right_line_strip, lane_bound_line_strip, center_line_strip; + visualization_msgs::msg::Marker left_line_strip, right_line_strip, start_bound_line_strip, center_line_strip; visualization::initLineStringMarker( &left_line_strip, "map", additional_namespace + "left_lane_bound", c); visualization::initLineStringMarker( &right_line_strip, "map", additional_namespace + "right_lane_bound", c); visualization::initLineStringMarker( - &lane_bound_line_strip, "map", additional_namespace + "lane_bound", c); + &start_bound_line_strip, "map", additional_namespace + "lane_start_bound", c); visualization::initLineStringMarker( ¢er_line_strip, "map", additional_namespace + "center_lane_line", c); @@ -894,10 +894,10 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra lanelet::ConstLineString3d left_ls = lll.leftBound(); lanelet::ConstLineString3d right_ls = lll.rightBound(); lanelet::ConstLineString3d center_ls = lll.centerline(); - lanelet::LineString3d lane_bound_ls(lanelet::utils::getId()); - lane_bound_ls.push_back(lanelet::Point3d( + lanelet::LineString3d start_bound_ls(lanelet::utils::getId()); + start_bound_ls.push_back(lanelet::Point3d( lanelet::utils::getId(), left_ls.front().x(), left_ls.front().y(), left_ls.front().z())); - lane_bound_ls.push_back(lanelet::Point3d( + start_bound_ls.push_back(lanelet::Point3d( lanelet::utils::getId(), right_ls.front().x(), right_ls.front().y(), right_ls.front().z())); if (!exists(added, left_ls.id())) { @@ -908,9 +908,9 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra visualization::pushLineStringMarker(&right_line_strip, right_ls, c, lss); added.insert(right_ls.id()); } - if (!exists(added, lane_bound_ls.id())) { - visualization::pushLineStringMarker(&lane_bound_line_strip, lane_bound_ls, c, lss); - added.insert(lane_bound_ls.id()); + if (!exists(added, start_bound_ls.id())) { + visualization::pushLineStringMarker(&start_bound_line_strip, start_bound_ls, c, lss); + added.insert(start_bound_ls.id()); } if (viz_centerline && !exists(added, center_ls.id())) { visualization::pushLineStringMarker(¢er_line_strip, center_ls, c, lss_center); @@ -928,8 +928,8 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra if (!center_line_strip.points.empty()) { marker_array.markers.push_back(center_line_strip); } - if (!lane_bound_line_strip.points.empty()) { - marker_array.markers.push_back(lane_bound_line_strip); + if (!start_bound_line_strip.points.empty()) { + marker_array.markers.push_back(start_bound_line_strip); } return marker_array; } From f85c6a879b35c92c3be3fe19759fc4ddb456dacc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 18 May 2022 12:19:13 +0000 Subject: [PATCH 3/3] ci(pre-commit): autofix --- map/lanelet2_extension/lib/visualization.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/map/lanelet2_extension/lib/visualization.cpp b/map/lanelet2_extension/lib/visualization.cpp index 441ab4c93e14f..a95a9aa1e49e0 100644 --- a/map/lanelet2_extension/lib/visualization.cpp +++ b/map/lanelet2_extension/lib/visualization.cpp @@ -878,7 +878,8 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra double lss_center = std::max(lss * 0.1, 0.02); std::unordered_set added; - visualization_msgs::msg::Marker left_line_strip, right_line_strip, start_bound_line_strip, center_line_strip; + visualization_msgs::msg::Marker left_line_strip, right_line_strip, start_bound_line_strip, + center_line_strip; visualization::initLineStringMarker( &left_line_strip, "map", additional_namespace + "left_lane_bound", c); visualization::initLineStringMarker(