From dba6de4f85261bbdda2370e8eec9e46b7b765ec6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 18 Jun 2024 00:54:00 +0900 Subject: [PATCH] feat(lanelet2_extension): overwriteLaneletsCenterline supports "waypoints" (#252) * feat(lanelet2_extension): centerline is converted to waypoints Signed-off-by: Takayuki Murooka * fix lanelet2_extension_python Signed-off-by: Takayuki Murooka * update README Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * early return Signed-off-by: Takayuki Murooka * fix clang-tidy Signed-off-by: Takayuki Murooka * Update tmp/lanelet2_extension/lib/utilities.cpp Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> * style(pre-commit): autofix * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../docs/lanelet2_format_extension.md | 20 +++++++ .../lanelet2_extension/utility/utilities.hpp | 10 ++++ tmp/lanelet2_extension/lib/utilities.cpp | 19 +++++++ .../test/src/test_utilities.cpp | 43 +++++++++++++++ tmp/lanelet2_extension_python/src/utility.cpp | 55 +++++++++++++------ 5 files changed, 129 insertions(+), 18 deletions(-) diff --git a/tmp/lanelet2_extension/docs/lanelet2_format_extension.md b/tmp/lanelet2_extension/docs/lanelet2_format_extension.md index 744fd128..6eca915b 100644 --- a/tmp/lanelet2_extension/docs/lanelet2_format_extension.md +++ b/tmp/lanelet2_extension/docs/lanelet2_format_extension.md @@ -479,3 +479,23 @@ _An example:_ ... ``` + +### Centerline + +Note that the following explanation is not related to the Lanelet2 map format but how the Autoware handles the centerline in the Lanelet2 map. + +Centerline is defined in Lanelet2 to guide the vehicle. By explicitly setting the centerline in the Lanelet2 map, the ego's planned path follows the centerline. +However, based on the current Autoware's usage of the centerline, there are several limitations. + +- The object's predicted path also follows the centerline. + - This may adversely affect the decision making of planning modules since the centerline is supposed to be used only by the ego's path planning. +- The coordinate transformation on the lane's frenet frame cannot be calculated correctly. + - For example, when the lateral distance between the actual road's centerline and a parked vehicle is calculated, actually the result will be the lateral distance between the (explicit) centerline and the vehicle. + +To solve above limitations, the `overwriteLaneletsCenterlineWithWaypoints` was implemented in addition to `overwriteLaneletsCenterline` where the centerline in all the lanes is calculated. + +- `overwriteLaneletsCenterlineWithWaypoints` + - The (explicit) centerline in the Lanelet2 map is converted to the new `waypoints` tag. This `waypoints` is only applied to the ego's path planning. + - Therefore, the above limitations can be solved, but the Autoware's usage of the centerline may be hard to understand. +- `overwriteLaneletsCenterline` + - The (explicit) centerline in the Lanelet2 map is used as it is. Easy to understand the Autoware's usage of the centerline, but we still have above limitations. diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp index 7df3f6a2..b9d068aa 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp @@ -59,6 +59,16 @@ void overwriteLaneletsCenterline( lanelet::LaneletMapPtr lanelet_map, const double resolution = 5.0, const bool force_overwrite = false); +/** + * @brief Apply another patch for centerline because the overwriteLaneletsCenterline + * has several limitations. See the following document in detail. + * https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#centerline + * // NOLINT + */ +void overwriteLaneletsCenterlineWithWaypoints( + lanelet::LaneletMapPtr lanelet_map, const double resolution = 5.0, + const bool force_overwrite = false); + lanelet::ConstLanelets getConflictingLanelets( const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet); diff --git a/tmp/lanelet2_extension/lib/utilities.cpp b/tmp/lanelet2_extension/lib/utilities.cpp index 8dbf5145..01085855 100644 --- a/tmp/lanelet2_extension/lib/utilities.cpp +++ b/tmp/lanelet2_extension/lib/utilities.cpp @@ -511,6 +511,25 @@ void overwriteLaneletsCenterline( } } +void overwriteLaneletsCenterlineWithWaypoints( + lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool force_overwrite) +{ + for (auto & lanelet_obj : lanelet_map->laneletLayer) { + if (force_overwrite) { + const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution); + lanelet_obj.setCenterline(fine_center_line); + } else { + if (lanelet_obj.hasCustomCenterline()) { + const auto & centerline = lanelet_obj.centerline(); + lanelet_obj.setAttribute("waypoints", centerline.id()); + } + + const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution); + lanelet_obj.setCenterline(fine_center_line); + } + } +} + lanelet::ConstLanelets getConflictingLanelets( const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet) { diff --git a/tmp/lanelet2_extension/test/src/test_utilities.cpp b/tmp/lanelet2_extension/test/src/test_utilities.cpp index 4d53c03c..65d0945b 100644 --- a/tmp/lanelet2_extension/test/src/test_utilities.cpp +++ b/tmp/lanelet2_extension/test/src/test_utilities.cpp @@ -87,6 +87,14 @@ class TestSuite : public ::testing::Test // NOLINT for gtest merging_lanelet.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Road; + // create sample lanelets + Point3d cp1 = Point3d(getId(), 0.5, 0., 0.); + Point3d cp2 = Point3d(getId(), 0.5, 0.5, 0.); + Point3d cp3 = Point3d(getId(), 0.5, 1., 0.); + + LineString3d ls_centerline(getId(), {cp1, cp2, cp3}); + road_lanelet.setCenterline(ls_centerline); + sample_map_ptr->add(road_lanelet); sample_map_ptr->add(next_lanelet); sample_map_ptr->add(next_lanelet2); @@ -104,12 +112,47 @@ class TestSuite : public ::testing::Test // NOLINT for gtest private: }; +TEST_F(TestSuite, OverwriteLaneletsCenterlineWithWaypoints) // NOLINT for gtest +{ + double resolution = 5.0; + bool force_overwrite = false; + + // memorize the original information of the centerline + std::unordered_map lanelet_centerline_map{}; + for (const auto & lanelet : sample_map_ptr->laneletLayer) { + if (lanelet.hasCustomCenterline()) { + lanelet_centerline_map.insert({lanelet.id(), lanelet.centerline().id()}); + } + } + + // convert centerline to waypoints + lanelet::utils::overwriteLaneletsCenterlineWithWaypoints( + sample_map_ptr, resolution, force_overwrite); + + for (const auto & lanelet : sample_map_ptr->laneletLayer) { + if (lanelet_centerline_map.find(lanelet.id()) != lanelet_centerline_map.end()) { + // check if the lanelet has waypoints. + ASSERT_TRUE(lanelet.hasAttribute("waypoints")) << "The lanelet does not have a waypoints."; + // check if the waypoints points to the linestring of the centerline. + ASSERT_TRUE( + lanelet.attribute("waypoints").asId().value() == lanelet_centerline_map.at(lanelet.id())) + << "The waypoint's ID is invalid."; + } + } + + // check if all the lanelets have a centerline + for (const auto & lanelet : sample_map_ptr->laneletLayer) { + ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline"; + } +} + TEST_F(TestSuite, OverwriteLaneletsCenterline) // NOLINT for gtest { double resolution = 5.0; bool force_overwrite = false; lanelet::utils::overwriteLaneletsCenterline(sample_map_ptr, resolution, force_overwrite); + // check if all the lanelets have a centerline for (const auto & lanelet : sample_map_ptr->laneletLayer) { ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline"; } diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp index fdd612f1..9d336c74 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -45,9 +45,8 @@ lanelet::Optional lineStringWithWidthToPolygon( lanelet::ConstPolygon3d poly{}; if (lanelet::utils::lineStringWithWidthToPolygon(linestring, &poly)) { return poly; - } else { - return {}; } + return {}; } lanelet::Optional lineStringToPolygon( @@ -56,9 +55,8 @@ lanelet::Optional lineStringToPolygon( lanelet::ConstPolygon3d poly{}; if (lanelet::utils::lineStringToPolygon(linestring, &poly)) { return poly; - } else { - return {}; } + return {}; } lanelet::ArcCoordinates getArcCoordinates( @@ -69,7 +67,9 @@ lanelet::ArcCoordinates getArcCoordinates( serialized_msg.reserve(message_header_length + pose_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size(); for (size_t i = 0; i < pose_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Pose pose; static rclcpp::Serialization serializer; @@ -84,7 +84,9 @@ double getLaneletAngle(const lanelet::ConstLanelet & lanelet, const std::string serialized_msg.reserve(message_header_length + point_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size(); for (size_t i = 0; i < point_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Point point; static rclcpp::Serialization serializer; @@ -100,7 +102,9 @@ bool isInLanelet( serialized_msg.reserve(message_header_length + pose_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size(); for (size_t i = 0; i < pose_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Pose pose; static rclcpp::Serialization serializer; @@ -116,7 +120,9 @@ std::vector getClosestCenterPose( serialized_point_msg.reserve(message_header_length + search_point_byte.size()); serialized_point_msg.get_rcl_serialized_message().buffer_length = search_point_byte.size(); for (size_t i = 0; i < search_point_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_point_msg.get_rcl_serialized_message().buffer[i] = search_point_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Point search_point; static rclcpp::Serialization serializer_point; @@ -137,7 +143,9 @@ double getLateralDistanceToCenterline( serialized_msg.reserve(message_header_length + pose_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size(); for (size_t i = 0; i < pose_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Pose pose; static rclcpp::Serialization serializer; @@ -153,7 +161,9 @@ double getLateralDistanceToClosestLanelet( serialized_msg.reserve(message_header_length + pose_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size(); for (size_t i = 0; i < pose_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Pose pose; static rclcpp::Serialization serializer; @@ -180,9 +190,8 @@ lanelet::Optional getLinkedLanelet( if (lanelet::utils::query::getLinkedLanelet( parking_space, all_road_lanelets, all_parking_lots, &linked_lanelet)) { return linked_lanelet; - } else { - return {}; } + return {}; } lanelet::Optional getLinkedLanelet( @@ -192,9 +201,8 @@ lanelet::Optional getLinkedLanelet( lanelet::ConstLanelet linked_lanelet; if (lanelet::utils::query::getLinkedLanelet(parking_space, lanelet_map_ptr, &linked_lanelet)) { return linked_lanelet; - } else { - return {}; } + return {}; } lanelet::Optional getLinkedParkingLot( @@ -203,9 +211,8 @@ lanelet::Optional getLinkedParkingLot( lanelet::ConstPolygon3d linked_parking_lot; if (lanelet::utils::query::getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) { return linked_parking_lot; - } else { - return {}; } + return {}; } lanelet::Optional getLinkedParkingLot( @@ -215,9 +222,8 @@ lanelet::Optional getLinkedParkingLot( if (lanelet::utils::query::getLinkedParkingLot( current_position, all_parking_lots, &linked_parking_lot)) { return linked_parking_lot; - } else { - return {}; } + return {}; } lanelet::Optional getLinkedParkingLot( @@ -228,9 +234,8 @@ lanelet::Optional getLinkedParkingLot( if (lanelet::utils::query::getLinkedParkingLot( parking_space, all_parking_lots, &linked_parking_lot)) { return linked_parking_lot; - } else { - return {}; } + return {}; } lanelet::ConstLanelets getLaneletsWithinRange_point( @@ -241,7 +246,9 @@ lanelet::ConstLanelets getLaneletsWithinRange_point( serialized_msg.reserve(message_header_length + point_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size(); for (size_t i = 0; i < point_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Point point; static rclcpp::Serialization serializer; @@ -258,7 +265,9 @@ lanelet::ConstLanelets getLaneChangeableNeighbors_point( serialized_msg.reserve(message_header_length + point_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size(); for (size_t i = 0; i < point_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Point point; static rclcpp::Serialization serializer; @@ -275,7 +284,9 @@ lanelet::ConstLanelets getAllNeighbors_point( serialized_msg.reserve(message_header_length + point_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size(); for (size_t i = 0; i < point_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Point point; static rclcpp::Serialization serializer; @@ -291,7 +302,9 @@ lanelet::Optional getClosestLanelet( serialized_msg.reserve(message_header_length + pose_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size(); for (size_t i = 0; i < pose_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Pose pose; static rclcpp::Serialization serializer; @@ -299,9 +312,8 @@ lanelet::Optional getClosestLanelet( lanelet::ConstLanelet closest_lanelet{}; if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { return closest_lanelet; - } else { - return {}; } + return {}; } lanelet::Optional getClosestLaneletWithConstrains( @@ -314,7 +326,9 @@ lanelet::Optional getClosestLaneletWithConstrains( serialized_msg.reserve(message_header_length + pose_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size(); for (size_t i = 0; i < pose_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Pose pose; static rclcpp::Serialization serializer; @@ -323,9 +337,8 @@ lanelet::Optional getClosestLaneletWithConstrains( if (lanelet::utils::query::getClosestLaneletWithConstrains( lanelets, pose, &closest_lanelet, dist_threshold, yaw_threshold)) { return closest_lanelet; - } else { - return {}; } + return {}; } lanelet::ConstLanelets getCurrentLanelets_point( @@ -336,7 +349,9 @@ lanelet::ConstLanelets getCurrentLanelets_point( serialized_msg.reserve(message_header_length + point_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size(); for (size_t i = 0; i < point_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Point point; static rclcpp::Serialization serializer; @@ -354,7 +369,9 @@ lanelet::ConstLanelets getCurrentLanelets_pose( serialized_msg.reserve(message_header_length + pose_byte.size()); serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size(); for (size_t i = 0; i < pose_byte.size(); ++i) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic) serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i]; + // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) } geometry_msgs::msg::Pose pose; static rclcpp::Serialization serializer; @@ -368,6 +385,7 @@ lanelet::ConstLanelets getCurrentLanelets_pose( // for handling functions with default arguments /// utilities.cpp +// NOLINTBEGIN BOOST_PYTHON_FUNCTION_OVERLOADS( generateFineCenterline_overload, lanelet::utils::generateFineCenterline, 1, 2) BOOST_PYTHON_FUNCTION_OVERLOADS( @@ -387,6 +405,7 @@ BOOST_PYTHON_FUNCTION_OVERLOADS( getClosestLaneletWithConstrains_overload, ::getClosestLaneletWithConstrains, 2, 4) BOOST_PYTHON_FUNCTION_OVERLOADS( getPrecedingLaneletSequences_overload, lanelet::utils::query::getPrecedingLaneletSequences, 3, 4) +// NOLINTEND BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) {