diff --git a/tmp/lanelet2_extension_python/example/example.py b/tmp/lanelet2_extension_python/example/example.py index d8557fc4..5db2cf4c 100644 --- a/tmp/lanelet2_extension_python/example/example.py +++ b/tmp/lanelet2_extension_python/example/example.py @@ -1,5 +1,3 @@ -import math - from geometry_msgs.msg import Point from geometry_msgs.msg import Pose import lanelet2 @@ -42,44 +40,47 @@ def test_utility_query(lanelet_map, routing_graph): print(f"""{len(query.getAllParkingSpaces(lanelet_map))=}""") print(f"""{len(query.getAllFences(lanelet_map))=}""") print(f"""{len(query.getAllPedestrianMarkings(lanelet_map))=}""") - print(f"""{len(query.getLinkedParkingSpaces(lanelet56, lanelet_map))=}""") - print( - f"""{len(query.getLinkedParkingSpaces(lanelet56, query.getAllParkingSpaces(lanelet_map), query.getAllParkingLots(lanelet_map)))=}""" - ) print(f"""{len(query.stopLinesLanelets(lanelets))=}""") print(f"""{len(query.stopLinesLanelet(lanelet108))=}""") print(f"""{len(query.stopSignStopLines(lanelets))=}""") - lanelet56_centerline_basic_points = [p.basicPoint() for p in lanelet56.centerline] - lanelet56_centerline_center = np.sum(lanelet56_centerline_basic_points) * ( - 1.0 / len(lanelet56_centerline_basic_points) + lanelet56_centerline_center = np.sum([p.basicPoint() for p in lanelet56.centerline]) * ( + 1.0 / len(lanelet56.centerline) + ) + search_point = Point( + x=lanelet56_centerline_center.x, + y=lanelet56_centerline_center.y, + z=lanelet56_centerline_center.z, ) - search_point = Point() - search_point.x = lanelet56_centerline_center.x - search_point.y = lanelet56_centerline_center.y - search_point.z = lanelet56_centerline_center.z + print(f"""{search_point=}""") print(f"""{[ll2.id for ll2 in query.getLaneletsWithinRange(lanelets, search_point, 15.0)]=}""") - search_point_2d = lanelet2.core.BasicPoint2d() - search_point_2d.x = lanelet56_centerline_center.x - search_point_2d.y = lanelet56_centerline_center.y + search_point_2d = lanelet2.core.BasicPoint2d( + x=lanelet56_centerline_center.x, y=lanelet56_centerline_center.y + ) print( f"""{[ll2.id for ll2 in query.getLaneletsWithinRange(lanelets, search_point_2d, 15.0)]=}""" ) print(f"""{[ll2.id for ll2 in query.getLaneChangeableNeighbors(routing_graph, lanelet108)]=}""") + print( + f"""{[ll2.id for ll2 in query.getLaneChangeableNeighbors(routing_graph, query.roadLanelets(lanelets), search_point)]=}""" + ) print(f"""{[ll2.id for ll2 in query.getAllNeighbors(routing_graph, lanelet56)]=}""") + print( + f"""{[ll2.id for ll2 in query.getAllNeighbors(routing_graph, query.roadLanelets(lanelets), search_point)]=}""" + ) print(f"""{[ll2.id for ll2 in query.getAllNeighborsLeft(routing_graph, lanelet56)]=}""") print(f"""{[ll2.id for ll2 in query.getAllNeighborsRight(routing_graph, lanelet56)]=}""") search_pose = Pose() search_pose.position = search_point - temp_lanelet = lanelet2.core.Lanelet(0, lanelet56.leftBound, lanelet56.rightBound) - if ( - query.getClosestLaneletWithConstrains(lanelets, search_pose, temp_lanelet, 10.0, math.pi) - and temp_lanelet is not None - ): - print(f"""{temp_lanelet.id}""") - # lanelet::ConstLaneletsへのpointerだからこの関数は無理 - temp_lanelets = [] - if query.getCurrentLanelets(lanelets, search_point, temp_lanelets): - print(f"""{[ll2.id for ll2 in temp_lanelets]}""") + print( + f"""{(query.getClosestLanelet(lanelets, search_pose).id if query.getClosestLanelet(lanelets, search_pose) else None)=}""" + ) + print(f"""{[ll2.id for ll2 in query.getCurrentLanelets(lanelets, search_point)]=}""") + print( + f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getSucceedingLaneletSequences(routing_graph, lanelet108, 100.0)]}""" + ) + print( + f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getPrecedingLaneletSequences(routing_graph, lanelet108, 100.0)]}""" + ) def test_utility_utilities(): diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py index 70738c12..7a245b46 100644 --- a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py @@ -27,19 +27,10 @@ getAllFences = _utility_cpp.getAllFences getAllPedestrianMarkings = _utility_cpp.getAllPedestrianMarkings getAllParkingSpaces = _utility_cpp.getAllParkingSpaces - -# TODO(Mamoru Sobue): how to dispatch overloads getLinkedParkingSpaces = _utility_cpp.getLinkedParkingSpaces - -# TODO(Mamoru Sobue): how to dispatch overloads getLinkedLanelet = _utility_cpp.getLinkedLanelet - -# TODO(Mamoru Sobue): how to dispatch overloads getLinkedLanelets = _utility_cpp.getLinkedLanelets - -# TODO(Mamoru Sobue): how to dispatch overloads getLinkedParkingLot = _utility_cpp.getLinkedParkingLot - stopLinesLanelets = _utility_cpp.stopLinesLanelets stopLinesLanelet = _utility_cpp.stopLinesLanelet stopSignStopLines = _utility_cpp.stopSignStopLines @@ -59,7 +50,7 @@ def getLaneChangeableNeighbors(*args): return _utility_cpp.getLaneChangeableNeighbors(args[0], args[1]) if len(args) == 3 and isinstance(args[2], Point): point_byte = serialize_message(args[2]) - return _utility_cpp.getLaneChangeableNeighbor_point(args[0], args[1], point_byte) + return _utility_cpp.getLaneChangeableNeighbors_point(args[0], args[1], point_byte) raise TypeError("argument number does not match or 3rd argument is not Point type") @@ -68,13 +59,18 @@ def getAllNeighbors(*args): return _utility_cpp.getAllNeighbors(args[0], args[1]) if len(args) == 3 and isinstance(args[2], Point): point_byte = serialize_message(args[2]) - return _utility_cpp.getAllNeighbors(args[0], args[1], point_byte) + return _utility_cpp.getAllNeighbors_point(args[0], args[1], point_byte) getAllNeighborsLeft = _utility_cpp.getAllNeighborsLeft getAllNeighborsRight = _utility_cpp.getAllNeighborsRight +def getClosestLanelet(lanelets, pose: Pose): + pose_byte = serialize_message(pose) + return _utility_cpp.getClosestLanelet(lanelets, pose_byte) + + def getClosestLaneletWithConstrains( lanelets, pose: Pose, closest_lanelet, dist_threshold, yaw_threshold ): @@ -84,14 +80,14 @@ def getClosestLaneletWithConstrains( ) -def getCurrentLanelets(lanelets, point: Point, current_lanelets): +def getCurrentLanelets(lanelets, point: Point): if isinstance(point, Point): point_byte = serialize_message(point) - return _utility_cpp.getCurrentLanelets_point(lanelets, point_byte, current_lanelets) + return _utility_cpp.getCurrentLanelets_point(lanelets, point_byte) if isinstance(point, Pose): pose_byte = serialize_message(point) - return _utility_cpp.getCurrentLanelets_pose(lanelets, pose_byte, current_lanelets) - raise TypeError("argument number does not match or 3rd argument is not Pose type") + return _utility_cpp.getCurrentLanelets_pose(lanelets, pose_byte) + raise TypeError("argument number does not match or 2nd argument is not Point/Pose type") getSucceedingLaneletSequences = _utility_cpp.getSucceedingLaneletSequences diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py index 0dfb51e2..12031dc8 100644 --- a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py @@ -1,7 +1,8 @@ from geometry_msgs.msg import Point from geometry_msgs.msg import Pose +from geometry_msgs.msg import Quaternion +import lanelet2 import lanelet2_extension_python._lanelet2_extension_python_boost_python_utility as _utility_cpp -from rclpy.serialization import deserialize_message from rclpy.serialization import serialize_message combineLaneletsShape = _utility_cpp.combineLaneletsShape @@ -12,8 +13,29 @@ getExpandedLanelet = _utility_cpp.getExpandedLanelet getExpandedLanelets = _utility_cpp.getExpandedLanelets overwriteLaneletsCenterline = _utility_cpp.overwriteLaneletsCenterline -getLaneletLength2d = _utility_cpp.getLaneletLength2d -getLaneletLength3d = _utility_cpp.getLaneletLength3d +getConflictingLanelets = _utility_cpp.getConflictingLanelets +lineStringWithWidthToPolygon = _utility_cpp._utility_cpp.lineStringWithWidthToPolygon +lineStringToPolygon = _utility_cpp._utility_cpp.lineStringToPolygon + + +def getLaneletLength2d(*args): + if len(args) == 1 and isinstance(args[0], lanelet2.core.Lanelet): + return _utility_cpp.getLaneletLength2d(args[0]) + if len(args) == 1 and isinstance(args[0], list): + return _utility_cpp.getLaneletLength2d(args[0]) + raise TypeError( + "argument number does not match or 1st argument is not Lanelet or [Lanelet] type" + ) + + +def getLaneletLength3d(*args): + if len(args) == 1 and isinstance(args[0], lanelet2.core.Lanelet): + return _utility_cpp.getLaneletLength3d(args[0]) + if len(args) == 1 and isinstance(args[0], list): + return _utility_cpp.getLaneletLength3d(args[0]) + raise TypeError( + "argument number does not match or 1st argument is not Lanelet or [Lanelet] type" + ) def getArcCoordinates(lanelet_sequence, pose: Pose): @@ -38,21 +60,10 @@ def isInLanelet(pose: Pose, lanelet, radius=0.0): def getClosestCenterPose(lanelet, point: Point): # https://dev.to/pgradot/sharing-strings-between-c-and-python-through-byte-buffers-1nj0 point_byte = serialize_message(point) - pose_byte = _utility_cpp.getClosestCenterPose(lanelet, point_byte) - print(len(pose_byte)) # これは60ある - print(pose_byte[0].encode()) # これで b'\x00'と出た - byte_array = bytearray() - for i in range(len(pose_byte)): - # print(pose_byte[i]) - print( - type(pose_byte[i].encode("unicode-escape").decode("unicode-escape")), - pose_byte[i].encode("unicode-escape").decode("unicode-escape"), - ) - byte_array.append( - pose_byte[i].encode("unicode-escape").decode("unicode-escape") - ) # 0xb9で詰まる - print(byte_array) - return deserialize_message(byte_array, Pose) + pose_array = _utility_cpp.getClosestCenterPose(lanelet, point_byte) + pos = Point(x=pose_array[0], y=pose_array[1], z=pose_array[2]) + quat = Quaternion(x=pose_array[3], y=pose_array[4], z=pose_array[5], w=pose_array[6]) + return Pose(position=pos, quaternion=quat) def getLateralDistanceToCenterline(lanelet, pose: Pose): diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp index ae2c1c02..44dedad9 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -35,7 +35,32 @@ namespace bp = boost::python; namespace { -// for handling functions with ros message type + +/* + * utilities.cpp + */ +lanelet::Optional lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring) +{ + lanelet::ConstPolygon3d poly{}; + if (lanelet::utils::lineStringWithWidthToPolygon(linestring, &poly)) { + return poly; + } else { + return {}; + } +} + +lanelet::Optional lineStringToPolygon( + const lanelet::ConstLineString3d & linestring) +{ + lanelet::ConstPolygon3d poly{}; + if (lanelet::utils::lineStringToPolygon(linestring, &poly)) { + return poly; + } else { + return {}; + } +} + lanelet::ArcCoordinates getArcCoordinates( const lanelet::ConstLanelets & lanelet_sequence, const std::string & pose_byte) { @@ -83,7 +108,7 @@ bool isInLanelet( return lanelet::utils::isInLanelet(pose, lanelet, radius); } -std::vector getClosestCenterPose( +std::vector getClosestCenterPose( const lanelet::ConstLanelet & lanelet, const std::string & search_point_byte) { rclcpp::SerializedMessage serialized_point_msg; @@ -96,22 +121,12 @@ std::vector getClosestCenterPose( geometry_msgs::msg::Point search_point; static rclcpp::Serialization serializer_point; serializer_point.deserialize_message(&serialized_point_msg, &search_point); - std::cout << search_point.x << ", " << search_point.y << ", " << search_point.z << std::endl; - // ここまで正しい const geometry_msgs::msg::Pose pose = lanelet::utils::getClosestCenterPose(lanelet, search_point); - std::cout << pose.position.x << ", " << pose.position.y << ", " << pose.position.z << std::endl; - std::cout << pose.orientation.x << ", " << pose.orientation.y << ", " << pose.orientation.z - << ", " << pose.orientation.w << std::endl; - // serializationも間違っていないはずなのだが, - // ここ以降かutilities.pyのgetClosestCenterPoseが間違っている - static rclcpp::Serialization serializer_pose; - rclcpp::SerializedMessage serialized_pose_msg; - serializer_pose.serialize_message(&pose, &serialized_pose_msg); - std::vector pose_byte; - for (size_t i = 0; i < serialized_pose_msg.size(); ++i) { - pose_byte.push_back(serialized_pose_msg.get_rcl_serialized_message().buffer[i]); - } - return pose_byte; + // NOTE: it was difficult to return the deserialized pose_byte and seriealize the pose_byte on + // python-side. So this function returns [*position, *quaternion] as double array + const auto & xyz = pose.position; + const auto & quat = pose.orientation; + return std::vector({xyz.x, xyz.y, xyz.z, quat.x, quat.y, quat.z, quat.w}); } double getLateralDistanceToCenterline( @@ -146,6 +161,78 @@ double getLateralDistanceToClosestLanelet( return lanelet::utils::getLateralDistanceToClosestLanelet(lanelet_sequence, pose); } +/* + * query.cpp + */ + +lanelet::ConstLanelets subtypeLanelets( + const lanelet::ConstLanelets & lls, const std::string & subtype) +{ + return lanelet::utils::query::subtypeLanelets(lls, subtype.c_str()); +} + +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 +283,29 @@ lanelet::ConstLanelets getAllNeighbors_point( return lanelet::utils::query::getAllNeighbors(graph, road_lanelets, point); } -bool getClosestLaneletWithConstrains( +lanelet::Optional getClosestLanelet( + const lanelet::ConstLanelets & lanelets, const std::string & pose_byte) +{ + rclcpp::SerializedMessage serialized_msg; + static constexpr size_t message_header_length = 8u; + 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) { + serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i]; + } + geometry_msgs::msg::Pose pose; + static rclcpp::Serialization serializer; + serializer.deserialize_message(&serialized_msg, &pose); + lanelet::ConstLanelet closest_lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { + return closest_lanelet; + } else { + return {}; + } +} + +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 +319,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 +341,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,26 +359,14 @@ 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 subtypeLanelets( - const lanelet::ConstLanelets & lls, const std::string & subtype) -{ - return lanelet::utils::query::subtypeLanelets(lls, subtype.c_str()); + lanelet::ConstLanelets current_lanelets{}; + lanelet::utils::query::getCurrentLanelets(lanelets, pose, ¤t_lanelets); + return current_lanelets; } } // namespace // for handling functions with default arguments -/// query.cpp -BOOST_PYTHON_FUNCTION_OVERLOADS( - stopSignStopLines_overload, lanelet::utils::query::stopSignStopLines, 1, 2) -BOOST_PYTHON_FUNCTION_OVERLOADS( - getClosestLaneletWithConstrains_overload, ::getClosestLaneletWithConstrains, 3, 5) -BOOST_PYTHON_FUNCTION_OVERLOADS( - getPrecedingLaneletSequences_overload, lanelet::utils::query::getPrecedingLaneletSequences, 3, 4) - /// utilities.cpp BOOST_PYTHON_FUNCTION_OVERLOADS( generateFineCenterline_overload, lanelet::utils::generateFineCenterline, 1, 2) @@ -280,9 +380,19 @@ BOOST_PYTHON_FUNCTION_OVERLOADS( overwriteLaneletsCenterline_overload, lanelet::utils::overwriteLaneletsCenterline, 1, 3) BOOST_PYTHON_FUNCTION_OVERLOADS(isInLanelet_overload, ::isInLanelet, 2, 3) +/// query.cpp +BOOST_PYTHON_FUNCTION_OVERLOADS( + stopSignStopLines_overload, lanelet::utils::query::stopSignStopLines, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS( + getClosestLaneletWithConstrains_overload, ::getClosestLaneletWithConstrains, 2, 4) +BOOST_PYTHON_FUNCTION_OVERLOADS( + getPrecedingLaneletSequences_overload, lanelet::utils::query::getPrecedingLaneletSequences, 3, 4) + BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) { - // utilities.cpp + /* + * utilities.cpp + */ bp::def("combineLaneletsShape", lanelet::utils::combineLaneletsShape); bp::def( "generateFineCenterline", lanelet::utils::generateFineCenterline, @@ -301,6 +411,9 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) bp::def( "overwriteLaneletsCenterline", lanelet::utils::overwriteLaneletsCenterline, overwriteLaneletsCenterline_overload()); + bp::def("getConflictingLanelets", lanelet::utils::getConflictingLanelets); + bp::def("lineStringWithWidthToPolygon", ::lineStringWithWidthToPolygon); + bp::def("lineStringToPolygon", ::lineStringToPolygon); bp::def( "getLaneletLength2d", lanelet::utils::getLaneletLength2d); bp::def( @@ -314,13 +427,17 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) bp::def("getPolygonFromArcLength", lanelet::utils::getPolygonFromArcLength); bp::def("getLaneletAngle", ::getLaneletAngle); // depends on ros msg bp::def("isInLanelet", ::isInLanelet, isInLanelet_overload()); // depends ros msg - bp::class_>("bytes").def(bp::vector_indexing_suite>()); - bp::def("getClosestCenterPose", getClosestCenterPose); // depends ros msg + bp::def("getClosestCenterPose", ::getClosestCenterPose); // depends ros msg + // NOTE: required for the return-value of getClosestCenterPose + bp::class_>("[position, quarternion]") + .def(bp::vector_indexing_suite>()); bp::def("getLateralDistanceToCenterline", ::getLateralDistanceToCenterline); // depends ros msg bp::def( "getLateralDistanceToClosestLanelet", ::getLateralDistanceToClosestLanelet); // depends ros msg - // query.cpp + /* + * query.cpp + */ bp::def("laneletLayer", lanelet::utils::query::laneletLayer); bp::def("subtypeLanelets", ::subtypeLanelets); bp::def("crosswalkLanelets", lanelet::utils::query::crosswalkLanelets); @@ -363,19 +480,19 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) const lanelet::ConstLanelet &, const lanelet::ConstLineStrings3d &, const lanelet::ConstPolygons3d &)>( "getLinkedParkingSpaces", lanelet::utils::query::getLinkedParkingSpaces); - // NOTE: this causes RuntimeWarning for duplicate to-Python converter + // NOTE: required for iterating the return-value of getLinkedParkingSpaces/getAllParkingLots, but + // this causes RuntimeWarning for duplicate to-Python converter bp::class_("lanelet::ConstLineStrings3d") .def(bp::vector_indexing_suite()); 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 +500,22 @@ 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( + "getLinkedParkingSpaces", lanelet::utils::query::getLinkedParkingSpaces); + bp::def( + "getLinkedLanelets", lanelet::utils::query::getLinkedLanelets); bp::def("stopLinesLanelets", lanelet::utils::query::stopLinesLanelets); bp::def("stopLinesLanelet", lanelet::utils::query::stopLinesLanelet); bp::def( @@ -417,13 +541,18 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) const std::string &)>("getAllNeighbors_point", ::getAllNeighbors_point); // depends on ros msg bp::def("getAllNeighborsLeft", lanelet::utils::query::getAllNeighborsLeft); bp::def("getAllNeighborsRight", lanelet::utils::query::getAllNeighborsRight); + bp::def("getClosestLanelet", ::getClosestLanelet); // depends on ros msg bp::def( "getClosestLaneletWithConstrains", ::getClosestLaneletWithConstrains, getClosestLaneletWithConstrains_overload()); // depends on ros msg bp::def("getCurrentLanelets_point", ::getCurrentLanelets_point); // depends on ros msg bp::def("getCurrentLanelets_pose", ::getCurrentLanelets_pose); // depends on ros msg + // NOTE: this is required for iterating getCurrentLanelets return value directly bp::class_("lanelet::ConstLanelets") .def(bp::vector_indexing_suite()); + // NOTE: this is required for return-type of getsucceeding/PrecedingLaneletSequences + bp::class_>("std::vector") + .def(bp::vector_indexing_suite>()); bp::def("getSucceedingLaneletSequences", lanelet::utils::query::getSucceedingLaneletSequences); bp::def( "getPrecedingLaneletSequences", lanelet::utils::query::getPrecedingLaneletSequences,