From 880dc3a9d74aae6888ab8578350d4a1aef679b0c Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 12 Jan 2024 22:26:07 +0900 Subject: [PATCH] finished except for one Signed-off-by: Mamoru Sobue --- tmp/lanelet2_extension_python/src/utility.cpp | 123 ++++++++++++++---- 1 file changed, 95 insertions(+), 28 deletions(-) diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp index ae2c1c02..de51f899 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -146,6 +146,68 @@ double getLateralDistanceToClosestLanelet( return lanelet::utils::getLateralDistanceToClosestLanelet(lanelet_sequence, pose); } +lanelet::Optional getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstLanelets & all_road_lanelets, + const lanelet::ConstPolygons3d & all_parking_lots) +{ + lanelet::ConstLanelet linked_lanelet; + if (lanelet::utils::query::getLinkedLanelet( + parking_space, all_road_lanelets, all_parking_lots, &linked_lanelet)) { + return linked_lanelet; + } else { + return {}; + } +} + +lanelet::Optional getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstLanelet linked_lanelet; + if (lanelet::utils::query::getLinkedLanelet(parking_space, lanelet_map_ptr, &linked_lanelet)) { + return linked_lanelet; + } else { + return {}; + } +} + +lanelet::Optional getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots) +{ + lanelet::ConstPolygon3d linked_parking_lot; + if (lanelet::utils::query::getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) { + return linked_parking_lot; + } else { + return {}; + } +} + +lanelet::Optional getLinkedParkingLot( + const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots) +{ + lanelet::ConstPolygon3d linked_parking_lot; + if (lanelet::utils::query::getLinkedParkingLot( + current_position, all_parking_lots, &linked_parking_lot)) { + return linked_parking_lot; + } else { + return {}; + } +} + +lanelet::Optional getLinkedParkingLot( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstPolygons3d & all_parking_lots) +{ + lanelet::ConstPolygon3d linked_parking_lot; + if (lanelet::utils::query::getLinkedParkingLot( + parking_space, all_parking_lots, &linked_parking_lot)) { + return linked_parking_lot; + } else { + return {}; + } +} + lanelet::ConstLanelets getLaneletsWithinRange_point( const lanelet::ConstLanelets & lanelets, const std::string & point_byte, const double range) { @@ -196,9 +258,8 @@ lanelet::ConstLanelets getAllNeighbors_point( return lanelet::utils::query::getAllNeighbors(graph, road_lanelets, point); } -bool getClosestLaneletWithConstrains( +lanelet::Optional getClosestLaneletWithConstrains( const lanelet::ConstLanelets & lanelets, const std::string & pose_byte, - lanelet::ConstLanelet * closest_lanelet_ptr, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()) { @@ -212,13 +273,17 @@ bool getClosestLaneletWithConstrains( geometry_msgs::msg::Pose pose; static rclcpp::Serialization serializer; serializer.deserialize_message(&serialized_msg, &pose); - return lanelet::utils::query::getClosestLaneletWithConstrains( - lanelets, pose, closest_lanelet_ptr, dist_threshold, yaw_threshold); + lanelet::ConstLanelet closest_lanelet; + if (lanelet::utils::query::getClosestLaneletWithConstrains( + lanelets, pose, &closest_lanelet, dist_threshold, yaw_threshold)) { + return closest_lanelet; + } else { + return {}; + } } -bool getCurrentLanelets_point( - const lanelet::ConstLanelets & lanelets, const std::string & point_byte, - lanelet::ConstLanelets * current_lanelets_ptr) +lanelet::ConstLanelets getCurrentLanelets_point( + const lanelet::ConstLanelets & lanelets, const std::string & point_byte) { rclcpp::SerializedMessage serialized_msg; static constexpr size_t message_header_length = 8u; @@ -230,12 +295,13 @@ bool getCurrentLanelets_point( geometry_msgs::msg::Point point; static rclcpp::Serialization serializer; serializer.deserialize_message(&serialized_msg, &point); - return lanelet::utils::query::getCurrentLanelets(lanelets, point, current_lanelets_ptr); + lanelet::ConstLanelets current_lanelets; + lanelet::utils::query::getCurrentLanelets(lanelets, point, ¤t_lanelets); + return current_lanelets; } -bool getCurrentLanelets_pose( - const lanelet::ConstLanelets & lanelets, const std::string & pose_byte, - lanelet::ConstLanelets * current_lanelets_ptr) +lanelet::ConstLanelets getCurrentLanelets_pose( + const lanelet::ConstLanelets & lanelets, const std::string & pose_byte) { rclcpp::SerializedMessage serialized_msg; static constexpr size_t message_header_length = 8u; @@ -247,7 +313,9 @@ bool getCurrentLanelets_pose( geometry_msgs::msg::Pose pose; static rclcpp::Serialization serializer; serializer.deserialize_message(&serialized_msg, &pose); - return lanelet::utils::query::getCurrentLanelets(lanelets, pose, current_lanelets_ptr); + lanelet::ConstLanelets current_lanelets; + lanelet::utils::query::getCurrentLanelets(lanelets, pose, ¤t_lanelets); + return current_lanelets; } lanelet::ConstLanelets subtypeLanelets( @@ -263,7 +331,7 @@ lanelet::ConstLanelets subtypeLanelets( BOOST_PYTHON_FUNCTION_OVERLOADS( stopSignStopLines_overload, lanelet::utils::query::stopSignStopLines, 1, 2) BOOST_PYTHON_FUNCTION_OVERLOADS( - getClosestLaneletWithConstrains_overload, ::getClosestLaneletWithConstrains, 3, 5) + getClosestLaneletWithConstrains_overload, ::getClosestLaneletWithConstrains, 2, 4) BOOST_PYTHON_FUNCTION_OVERLOADS( getPrecedingLaneletSequences_overload, lanelet::utils::query::getPrecedingLaneletSequences, 3, 4) @@ -369,13 +437,12 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) bp::class_("lanelet::ConstPolygons3d") .def(bp::vector_indexing_suite()); - bp::def( const lanelet::ConstLineString3d &, const lanelet::ConstLanelets &, - const lanelet::ConstPolygons3d &, lanelet::ConstLanelet *)>( - "getLinkedLanelet", lanelet::utils::query::getLinkedLanelet); - bp::def("getLinkedLanelet", lanelet::utils::query::getLinkedLanelet); + const lanelet::ConstPolygons3d &)>("getLinkedLanelet", ::getLinkedLanelet); + bp::def( + const lanelet::ConstLineString3d &, const lanelet::LaneletMapConstPtr &)>( + "getLinkedLanelet", ::getLinkedLanelet); bp::def( @@ -383,15 +450,15 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) bp::def( "getLinkedLanelets", lanelet::utils::query::getLinkedLanelets); - bp::def( - "getLinkedParkingLot", lanelet::utils::query::getLinkedParkingLot); - bp::def( - "getLinkedParkingLot", lanelet::utils::query::getLinkedParkingLot); - bp::def("getLinkedParkingLot", lanelet::utils::query::getLinkedParkingLot); + bp::def( + const lanelet::ConstLanelet &, const lanelet::ConstPolygons3d &)>( + "getLinkedParkingLot", ::getLinkedParkingLot); + bp::def( + const lanelet::BasicPoint2d &, const lanelet::ConstPolygons3d &)>( + "getLinkedParkingLot", ::getLinkedParkingLot); + bp::def( + const lanelet::ConstLineString3d &, const lanelet::ConstPolygons3d &)>( + "getLinkedParkingLot", ::getLinkedParkingLot); bp::def("stopLinesLanelets", lanelet::utils::query::stopLinesLanelets); bp::def("stopLinesLanelet", lanelet::utils::query::stopLinesLanelet); bp::def(