From 8185dd07e70aecc984c73b6ffc80a627627b9937 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 19 Jun 2023 01:04:30 +0900 Subject: [PATCH 1/9] dealing with overloads Signed-off-by: Mamoru Sobue --- tmp/lanelet2_extension_python/.gitignore | 1 + tmp/lanelet2_extension_python/CMakeLists.txt | 31 ++ .../example/example.py | 110 +++++ .../lanelet2_extension_python/__init__.py | 0 .../impl/__init__.py | 0 .../projection/__init__.py | 3 + .../utility/__init__.py | 0 .../utility/utilities.py | 163 +++++++ tmp/lanelet2_extension_python/package.xml | 31 ++ .../src/geometry.cpp | 35 ++ .../src/projection.cpp | 49 +++ .../src/regulatory_elements.cpp | 298 +++++++++++++ tmp/lanelet2_extension_python/src/utility.cpp | 404 ++++++++++++++++++ 13 files changed, 1125 insertions(+) create mode 100644 tmp/lanelet2_extension_python/.gitignore create mode 100644 tmp/lanelet2_extension_python/CMakeLists.txt create mode 100644 tmp/lanelet2_extension_python/example/example.py create mode 100644 tmp/lanelet2_extension_python/lanelet2_extension_python/__init__.py create mode 100644 tmp/lanelet2_extension_python/lanelet2_extension_python/impl/__init__.py create mode 100644 tmp/lanelet2_extension_python/lanelet2_extension_python/projection/__init__.py create mode 100644 tmp/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py create mode 100644 tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py create mode 100644 tmp/lanelet2_extension_python/package.xml create mode 100644 tmp/lanelet2_extension_python/src/geometry.cpp create mode 100644 tmp/lanelet2_extension_python/src/projection.cpp create mode 100644 tmp/lanelet2_extension_python/src/regulatory_elements.cpp create mode 100644 tmp/lanelet2_extension_python/src/utility.cpp diff --git a/tmp/lanelet2_extension_python/.gitignore b/tmp/lanelet2_extension_python/.gitignore new file mode 100644 index 00000000..8ba839c5 --- /dev/null +++ b/tmp/lanelet2_extension_python/.gitignore @@ -0,0 +1 @@ +example/sample-map-planning diff --git a/tmp/lanelet2_extension_python/CMakeLists.txt b/tmp/lanelet2_extension_python/CMakeLists.txt new file mode 100644 index 00000000..2b9e0286 --- /dev/null +++ b/tmp/lanelet2_extension_python/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.14) +project(lanelet2_extension_python) + +find_package(autoware_cmake REQUIRED) +find_package(python_cmake_module REQUIRED) + +autoware_package() + +find_package(BoostPython REQUIRED) + +include_directories( + ${BoostPython_INCLUDE_DIRS} + +) +ament_python_install_package(${PROJECT_NAME}) + +set(CMAKE_SHARED_LIBRARY_PREFIX "") +ament_auto_add_library(_${PROJECT_NAME}_boost_python_utility SHARED + src/utility.cpp +) +ament_auto_add_library(_${PROJECT_NAME}_boost_python_projection SHARED + src/projection.cpp +) +ament_auto_add_library(_${PROJECT_NAME}_boost_python_regulatory_elements SHARED + src/regulatory_elements.cpp +) +install(TARGETS _${PROJECT_NAME}_boost_python_utility _${PROJECT_NAME}_boost_python_projection _${PROJECT_NAME}_boost_python_regulatory_elements + DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" +) + +ament_package() diff --git a/tmp/lanelet2_extension_python/example/example.py b/tmp/lanelet2_extension_python/example/example.py new file mode 100644 index 00000000..7f300165 --- /dev/null +++ b/tmp/lanelet2_extension_python/example/example.py @@ -0,0 +1,110 @@ +from geometry_msgs.msg import Pose +import lanelet2 +import lanelet2.geometry +from lanelet2_extension_python.projection import MGRSProjector +from lanelet2_extension_python.utility.utilities import getArcCoordinates +from lanelet2_extension_python.utility.utilities import getLaneletAngle +from lanelet2_extension_python.utility.utilities import getLateralDistanceToClosestLanelet +from lanelet2_extension_python.utility.utilities import isInLanelet +import numpy as np + + +def print_layer(layer, layerName): + print("IDs in " + layerName) + print(sorted([elem.id for elem in layer])) + + +# Download the file from +# https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#preparation +if __name__ == "__main__": + proj = MGRSProjector(lanelet2.io.Origin(0.0, 0.0)) + ll2_map = lanelet2.io.load( + "/home/mamorusobue/workspace/pilot-auto.xx1/src/autoware/common/tmp/lanelet2_extension_python/example/sample-map-planning/lanelet2_map.osm", + proj, + ) + + layers = { + # "Points": ll2_map.pointLayer, + # "Line Strings": ll2_map.lineStringLayer, + "Polygons": ll2_map.polygonLayer, + "Lanelets": ll2_map.laneletLayer, + "Areas": ll2_map.areaLayer, + # "Regulatory Elements": ll2_map.regulatoryElementLayer, + } + + for layer_name, layer in layers.items(): + print_layer(layer, layer_name) + + lanelet56 = ll2_map.laneletLayer.get(56) + + # regulatory element + # get associated traffic lights + lights = lanelet56.trafficLights() + for light in lights: + print(light.stopLine) + # get turn_direction + if "turn_direction" in lanelet56.attributes: + turn_direction = lanelet56.attributes["turn_direction"] + print(f"lanelet56 has {turn_direction} turn_direction value") + + # routing + traffic_rules = lanelet2.traffic_rules.create( + lanelet2.traffic_rules.Locations.Germany, + lanelet2.traffic_rules.Participants.Vehicle, + ) + graph = lanelet2.routing.RoutingGraph(ll2_map, traffic_rules) + # get conflicting lanelets + conflictings = graph.conflicting(lanelet56) + print(f"lanelet56 is conflicting with {[conflicting.id for conflicting in conflictings]}") + # get following lanelets + followings = graph.following(lanelet56) + print(f"lanelet56 is connected to {[following.id for following in followings]}") + previouses = graph.previous(lanelet56) + print(f"lanelet56 is followed by {[previous.id for previous in previouses]}") + + centerline = lanelet56.centerline + print( + f"The centerline of lanelet56 is of id {centerline.id} and contains {len(centerline)} points." + ) + # access to points + pts = [] + for p in centerline: + pts.append(p) + # length, area + print(f"The length centerline is {lanelet2.geometry.length(centerline)}.") + + basic_pts = [p.basicPoint() for p in pts] + center = np.sum(basic_pts) * (1.0 / len(basic_pts)) + + center_sd_frame = lanelet2.geometry.toArcCoordinates( + lanelet2.geometry.to2D(centerline), lanelet2.geometry.to2D(center) + ) + print( + f"""The center of centerline56 is ({center_sd_frame.distance}, {center_sd_frame.length}) in arc coordinates""" + ) + + # test utility + pose = Pose() + pose.position.x = center.x + pose.position.y = center.y + pose.position.z = center.z + print(f"pose = [{pose.position.x}, {pose.position.y}, {pose.position.z}]") + + arc_coord = getArcCoordinates([lanelet56], pose) + print(f"getArcCoordinates(lanelet56, pose) = {arc_coord.distance}, {arc_coord.length}") + + lanelet_angle = getLaneletAngle(lanelet56, pose.position) + print(lanelet_angle) + + print(f"isInLanelet(pose, lanelet56) = {isInLanelet(pose, lanelet56)}") + + """ + closest_center_pose = getClosestCenterPose(lanelet56, pose.position) + print( + f"getClosestCenterPose(lanelet56, pose.position) = ({closest_center_pose})" + ) + """ + + print( + f"getLateralDistanceToClosestLanelet(lanelet56, pose) = {getLateralDistanceToClosestLanelet([lanelet56], pose)}" + ) diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/__init__.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/impl/__init__.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/impl/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/projection/__init__.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/projection/__init__.py new file mode 100644 index 00000000..b5e7067b --- /dev/null +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/projection/__init__.py @@ -0,0 +1,3 @@ +import lanelet2_extension_python._lanelet2_extension_python_boost_python_projection as _projection_cpp + +MGRSProjector = _projection_cpp.MGRSProjector diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py new file mode 100644 index 00000000..1e0b6718 --- /dev/null +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py @@ -0,0 +1,163 @@ +from typing import overload + +from geometry_msgs.msg import Point +from geometry_msgs.msg import Pose +from lanelet2.core import BasicPoint2d +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 +generateFineCenterline = _utility_cpp.generateFineCenterline +getCenterlineWithOffset = _utility_cpp.getCenterlineWithOffset +getRightBoundWithOffset = _utility_cpp.getRightBoundWithOffset +getLeftBoundWithOffset = _utility_cpp.getLeftBoundWithOffset +getExpandedLanelet = _utility_cpp.getExpandedLanelet +getExpandedLanelets = _utility_cpp.getExpandedLanelets +overwriteLaneletsCenterline = _utility_cpp.overwriteLaneletsCenterline +getLaneletLength2d = _utility_cpp.getLaneletLength2d +getLaneletLength3d = _utility_cpp.getLaneletLength3d + + +def getArcCoordinates(lanelet_sequence, pose: Pose): + pose_byte = serialize_message(pose) + return _utility_cpp.getArcCoordinates(lanelet_sequence, pose_byte) + + +getClosestSegment = _utility_cpp.getClosestSegment +getPolygonFromArcLength = _utility_cpp.getPolygonFromArcLength + + +def getLaneletAngle(lanelet, point: Point): + point_byte = serialize_message(point) + return _utility_cpp.getLaneletAngle(lanelet, point_byte) + + +def isInLanelet(pose: Pose, lanelet, radius=0.0): + pose_byte = serialize_message(pose) + return _utility_cpp.isInLanelet(pose_byte, lanelet, radius) + + +""" +TODO +def getClosestCenterPose(lanelet, point: Point): + point_byte = serialize_message(point) + pose_byte = _utility_cpp.getClosestCenterPose(lanelet, point_byte) + bytes = [] + for i in range(len(pose_byte)): + bytes.append(pose_byte[i].encode("latin1")) + print(bytes) + return deserialize_message(bytes, Pose) +""" + + +def getLateralDistanceToCenterline(lanelet, pose: Pose): + pose_byte = serialize_message(pose) + return _utility_cpp.getLateralDistanceToCenterline(lanelet, pose_byte) + + +def getLateralDistanceToClosestLanelet(lanelet_sequence, pose: Pose): + pose_byte = serialize_message(pose) + return _utility_cpp.getLateralDistanceToClosestLanelet(lanelet_sequence, pose_byte) + + +laneletLayer = _utility_cpp.laneletLayer +subtypeLanelets = _utility_cpp.subtypeLanelets +crosswalkLanelets = _utility_cpp.crosswalkLanelets +walkwayLanelets = _utility_cpp.walkwayLanelets +roadLanelets = _utility_cpp.roadLanelets +shoulderLanelets = _utility_cpp.shoulderLanelets +trafficLights = _utility_cpp.trafficLights +autowareTrafficLights = _utility_cpp.autowareTrafficLights +noStoppingAreas = _utility_cpp.noStoppingAreas +noParkingAreas = _utility_cpp.noParkingAreas +speedBumps = _utility_cpp.speedBumps +crosswalks = _utility_cpp.crosswalks +curbstones = _utility_cpp.curbstones +getAllPolygonsByType = _utility_cpp.getAllPolygonsByType +getAllObstaclePolygons = _utility_cpp.getAllObstaclePolygons +getAllParkingLots = _utility_cpp.getAllParkingLots +getAllPartitions = _utility_cpp.getAllPartitions +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 +help(getLinkedLanelets) + +# TODO(Mamoru Sobue): how to dispatch overloads +getLinkedParkingLot = _utility_cpp.getLinkedParkingLot + +stopLinesLanelets = _utility_cpp.stopLinesLanelets +stopLinesLanelet = _utility_cpp.stopLinesLanelet +stopSignStopLines = _utility_cpp.stopSignStopLines + + +@overload +def getLaneletsWithinRange(lanelets, point: BasicPoint2d, rng: float): + return _utility_cpp.getLaneletsWithinRange(lanelets, point, rng) + + +@overload +def getLaneletsWithinRange(lanelets, point: Point, rng: float): + point_byte = serialize_message(point) + return _utility_cpp.getLaneletsWithinRange_point(lanelets, point_byte, rng) + + +@overload +def getLaneChangeableNeighbors(routing_graph, lanelet): + return _utility_cpp.getLaneChangeableNeighbor(routing_graph, lanelet) + + +@overload +def getLaneChangeableNeighbors(routing_graph, lanelets, point: Point): + point_byte = serialize_message(point) + return _utility_cpp.getLaneChangeableNeighbor_point(routing_graph, lanelets, point_byte) + + +@overload +def getAllNeighbors(routing_graph, lanelet): + return _utility_cpp.getAllNeighbors(routing_graph, lanelet) + + +@overload +def getAllNeighbors(routing_graph, lanelets, point: Point): + point_byte = serialize_message(point) + return _utility_cpp.getAllNeighbors(routing_graph, lanelets, point_byte) + + +getAllNeighborsLeft = _utility_cpp.getAllNeighborsLeft +getAllNeighborsRight = _utility_cpp.getAllNeighborsRight + + +def getClosestLaneletWithConstrains( + lanelets, pose: Pose, closest_lanelet, dist_threshold, yaw_threshold +): + pose_byte = serialize_message(pose) + return _utility_cpp.getClosestLaneletWithConstrains( + lanelets, pose_byte, closest_lanelet, dist_threshold, yaw_threshold + ) + + +@overload +def getCurrentLanelets(point: Point): + point_byte = serialize_message(point) + return _utility_cpp.getCurrentLanelets_point(point_byte) + + +@overload +def getCurrentLanelets(pose: Pose): + pose_byte = serialize_message(pose) + return _utility_cpp.getCurrentLanelets_pose(pose_byte) + + +getSucceedingLaneletSequences = _utility_cpp.getSucceedingLaneletSequences +getPrecedingLaneletSequences = _utility_cpp.getPrecedingLaneletSequences diff --git a/tmp/lanelet2_extension_python/package.xml b/tmp/lanelet2_extension_python/package.xml new file mode 100644 index 00000000..d6444e48 --- /dev/null +++ b/tmp/lanelet2_extension_python/package.xml @@ -0,0 +1,31 @@ + + + + lanelet2_extension_python + 0.1.0 + The lanelet2_extension_python package contains Python bindings for lanelet2_extension package + Mamoru Sobue + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + python_cmake_module + + geometry_msgs + lanelet2_core + lanelet2_extension + lanelet2_io + lanelet2_projection + lanelet2_python + lanelet2_routing + lanelet2_traffic_rules + lanelet2_validation + libboost-python-dev + rclcpp + + ament_cmake_ros + + + ament_cmake + + diff --git a/tmp/lanelet2_extension_python/src/geometry.cpp b/tmp/lanelet2_extension_python/src/geometry.cpp new file mode 100644 index 00000000..f467446e --- /dev/null +++ b/tmp/lanelet2_extension_python/src/geometry.cpp @@ -0,0 +1,35 @@ +// Copyright 2023 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Mamoru Sobue + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include + +BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) +{ + namespace bp = boost::python; + // NOTE: should be added to Lanelet2 mainstream + // missing method of lanelet2_python/python_api/geometry.cpp + // def("area", boost::geometry::area); + bp::def("area", boost::geometry::area); + // def("area", boost::geometry::area); + bp::def("area", boost::geometry::area); +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension_python/src/projection.cpp b/tmp/lanelet2_extension_python/src/projection.cpp new file mode 100644 index 00000000..777ebe38 --- /dev/null +++ b/tmp/lanelet2_extension_python/src/projection.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Mamoru Sobue + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include + +#include + +#include + +BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_projection) +{ + namespace bp = boost::python; + + bp::class_>( + "Projector", "Projects point from lat/lon to x/y and back", bp::no_init) + .def("forward", &lanelet::Projector::forward, "Convert lat/lon into x/y") + .def("reverse", &lanelet::Projector::reverse, "Convert x/y into lat/lon") + .def( + "origin", &lanelet::Projector::origin, "Global origin of the converter", + bp::return_internal_reference<>()); + bp::class_< + lanelet::projection::MGRSProjector, std::shared_ptr, + bp::bases>("MGRSProjector", bp::init("origin")); + bp::class_< + lanelet::projection::TransverseMercatorProjector, + std::shared_ptr, + bp::bases>( + "TransverseMercatorProjector", bp::init("origin")); +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension_python/src/regulatory_elements.cpp b/tmp/lanelet2_extension_python/src/regulatory_elements.cpp new file mode 100644 index 00000000..c44af9c3 --- /dev/null +++ b/tmp/lanelet2_extension_python/src/regulatory_elements.cpp @@ -0,0 +1,298 @@ +// Copyright 2023 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Mamoru Sobue + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace bp = boost::python; + +// from lanelet2_python/src/core.cpp + +void format_helper([[maybe_unused]] std::ostream & os) +{ +} + +template +void format_helper(std::ostream & os, const std::string & s, const Args &... Args_) +{ + if (!s.empty()) { + os << ", "; + } + os << s; + format_helper(os, Args_...); +} + +template +void format_helper(std::ostream & os, const T & next, const Args &... Args_) +{ + os << ", "; + os << next; + format_helper(os, Args_...); +} + +template +void format(std::ostream & os, const T & first, const Args &... Args_) +{ + os << first; + format_helper(os, Args_...); +} + +template +std::string make_repr(const char * name, const Args &... Args_) +{ + std::ostringstream os; + os << name << '('; + format(os, Args_...); + os << ')'; + return os.str(); +} + +std::string repr(const bp::object & o) +{ + bp::object repr = bp::import("builtins").attr("repr"); + return bp::call(repr.ptr(), o); +} + +std::string repr(const lanelet::AttributeMap & a) +{ + if (a.empty()) { + return {}; + } + return repr(bp::object(a)); +} + +std::string repr(const lanelet::RegulatoryElementConstPtrs & regulatory_elements) +{ + if (regulatory_elements.empty()) { + return {}; + } + return repr(bp::list(regulatory_elements)); +} + +BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_regulatory_elements) +{ + // autoware_traffic_light + bp::class_< + lanelet::autoware::AutowareTrafficLight, boost::noncopyable, + std::shared_ptr, bp::bases>( + "AutowareTrafficLight", "autoware traffic light regulatory element", bp::no_init) + .def( + "__init__", bp::make_constructor( + &lanelet::autoware::AutowareTrafficLight::make, bp::default_call_policies(), + (bp::arg("Id"), bp::arg("attributes"), bp::arg("trafficLights"), + bp::arg("stopline") = lanelet::Optional(), + bp::arg("lightBulbs") = lanelet::LineString3d({})))) + .def( + "lightBulbs", + +[](lanelet::autoware::AutowareTrafficLight & self) { return self.lightBulbs(); }) + .def("addLightBulbs", &lanelet::autoware::AutowareTrafficLight::addLightBulbs) + .def("removeLightBulbs", &lanelet::autoware::AutowareTrafficLight::removeLightBulbs) + .def( + "__repr__", +[](lanelet::autoware::AutowareTrafficLight & r) { + return make_repr( + "AutowareTrafficLight", r.id(), repr(bp::dict(r.constData()->parameters)), + repr(r.attributes())); + }); + bp::implicitly_convertible< + std::shared_ptr, lanelet::RegulatoryElementPtr>(); + + // crosswalk + bp::class_< + lanelet::autoware::Crosswalk, boost::noncopyable, std::shared_ptr, + bp::bases>( + "Crosswalk", "autoware crosswalk regulatory element", bp::no_init) + .def( + "__init__", bp::make_constructor( + &lanelet::autoware::Crosswalk::make, bp::default_call_policies(), + (bp::arg("Id"), bp::arg("attributes"), bp::arg("crosswalk_lanelet"), + bp::arg("crosswalk_area"), bp::arg("stop_line")))) + .def( + "crosswalkAreas", +[](lanelet::autoware::Crosswalk & self) { return self.crosswalkAreas(); }) + .def( + "stopLines", +[](lanelet::autoware::Crosswalk & self) { return self.stopLines(); }) + .def( + "crosswalkLanelet", + +[](lanelet::autoware::Crosswalk & self) { return self.crosswalkLanelet(); }) + .def("addCrosswalkArea", &lanelet::autoware::Crosswalk::addCrosswalkArea) + .def("removeCrosswalkArea", &lanelet::autoware::Crosswalk::removeCrosswalkArea) + .def( + "__repr__", +[](lanelet::autoware::Crosswalk & r) { + return make_repr( + "Crosswalk", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes())); + }); + bp::implicitly_convertible< + std::shared_ptr, lanelet::RegulatoryElementPtr>(); + + // detection_area + bp::class_< + lanelet::autoware::DetectionArea, boost::noncopyable, + std::shared_ptr, bp::bases>( + "DetectionArea", "detection_area regulatory element", bp::no_init) + .def( + "__init__", + bp::make_constructor( + &lanelet::autoware::DetectionArea::make, bp::default_call_policies(), + (bp::arg("Id"), bp::arg("attributes"), bp::arg("detectionAreas"), bp::arg("stopLine")))) + .def( + "detectionAreas", + +[](lanelet::autoware::DetectionArea & self) { return self.detectionAreas(); }) + .def("addDetectionArea", &lanelet::autoware::DetectionArea::addDetectionArea) + .def("removeDetectionArea", &lanelet::autoware::DetectionArea::removeDetectionArea) + .def( + "stopLine", +[](lanelet::autoware::DetectionArea & self) { return self.stopLine(); }) + .def("setStopLine", &lanelet::autoware::DetectionArea::setStopLine) + .def("removeStopLine", &lanelet::autoware::DetectionArea::removeStopLine) + .def( + "__repr__", +[](lanelet::autoware::DetectionArea & r) { + return make_repr( + "DetectionArea", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes())); + }); + bp::implicitly_convertible< + std::shared_ptr, lanelet::RegulatoryElementPtr>(); + + // no_parking_area + bp::class_< + lanelet::autoware::NoParkingArea, boost::noncopyable, + std::shared_ptr, bp::bases>( + "NoParkingArea", "no_parking_area regulatory element", bp::no_init) + .def( + "__init__", bp::make_constructor( + &lanelet::autoware::NoParkingArea::make, bp::default_call_policies(), + (bp::arg("Id"), bp::arg("attributes"), bp::arg("no_parking_areas")))) + .def( + "noParkingAreas", + +[](lanelet::autoware::NoParkingArea & self) { return self.noParkingAreas(); }) + .def("addNoParkingArea", &lanelet::autoware::NoParkingArea::addNoParkingArea) + .def("removeNoParkingArea", &lanelet::autoware::NoParkingArea::removeNoParkingArea) + .def( + "__repr__", +[](lanelet::autoware::NoParkingArea & r) { + return make_repr( + "NoParkingArea", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes())); + }); + bp::implicitly_convertible< + std::shared_ptr, lanelet::RegulatoryElementPtr>(); + + // no_stopping_area + bp::class_< + lanelet::autoware::NoStoppingArea, boost::noncopyable, + std::shared_ptr, bp::bases>( + "NoStoppingArea", "no_stopping_area regulatory element", bp::no_init) + .def( + "__init__", bp::make_constructor( + &lanelet::autoware::NoStoppingArea::make, bp::default_call_policies(), + (bp::arg("Id"), bp::arg("attributes"), bp::arg("no_stopping_areas"), + bp::arg("stopLine") = lanelet::Optional()))) + .def( + "noStoppingAreas", + +[](lanelet::autoware::NoStoppingArea & self) { return self.noStoppingAreas(); }) + .def("addNoStoppingArea", &lanelet::autoware::NoStoppingArea::addNoStoppingArea) + .def("removeNoStoppingArea", &lanelet::autoware::NoStoppingArea::removeNoStoppingArea) + .def( + "stopLine", +[](lanelet::autoware::NoStoppingArea & self) { return self.stopLine(); }) + .def("setStopLine", &lanelet::autoware::NoStoppingArea::setStopLine) + .def("removeStopLine", &lanelet::autoware::NoStoppingArea::removeStopLine) + .def( + "__repr__", +[](lanelet::autoware::NoParkingArea & r) { + return make_repr( + "NoParkingArea", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes())); + }); + bp::implicitly_convertible< + std::shared_ptr, lanelet::RegulatoryElementPtr>(); + + // road_marking + bp::class_< + lanelet::autoware::RoadMarking, boost::noncopyable, + std::shared_ptr, bp::bases>( + "RoadMarking", "road_marking regulatory element", bp::no_init) + .def( + "__init__", bp::make_constructor( + &lanelet::autoware::RoadMarking::make, bp::default_call_policies(), + (bp::arg("Id"), bp::arg("attributes"), bp::arg("road_marking")))) + .def( + "roadMarking", +[](lanelet::autoware::RoadMarking & self) { return self.roadMarking(); }) + .def("setRoadMarking", &lanelet::autoware::RoadMarking::setRoadMarking) + .def("removeRoadMarking", &lanelet::autoware::RoadMarking::removeRoadMarking) + .def( + "__repr__", +[](lanelet::autoware::RoadMarking & r) { + return make_repr( + "RoadMarking", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes())); + }); + bp::implicitly_convertible< + std::shared_ptr, lanelet::RegulatoryElementPtr>(); + + // speed_bump + bp::class_< + lanelet::autoware::SpeedBump, boost::noncopyable, std::shared_ptr, + bp::bases>( + "SpeedBump", "speed_bump regulatory element", bp::no_init) + .def( + "__init__", bp::make_constructor( + &lanelet::autoware::SpeedBump::make, bp::default_call_policies(), + (bp::arg("Id"), bp::arg("attributes"), bp::arg("speed_bump")))) + .def( + "speedBump", +[](lanelet::autoware::SpeedBump & self) { return self.speedBump(); }) + .def("addSpeedBump", &lanelet::autoware::SpeedBump::addSpeedBump) + .def("removeSpeedBump", &lanelet::autoware::SpeedBump::removeSpeedBump) + .def( + "__repr__", +[](lanelet::autoware::SpeedBump & r) { + return make_repr( + "SpeedBump", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes())); + }); + bp::implicitly_convertible< + std::shared_ptr, lanelet::RegulatoryElementPtr>(); + + // virtual_traffic_light + bp::class_< + lanelet::autoware::VirtualTrafficLight, boost::noncopyable, + std::shared_ptr, bp::bases>( + "VirtualTrafficLight", "virtual_traffic_light regulatory element", bp::no_init) + .def( + "__init__", bp::make_constructor( + &lanelet::autoware::VirtualTrafficLight::make, bp::default_call_policies(), + (bp::arg("Id"), bp::arg("attributes"), bp::arg("virtual_traffic_light")))) + .def( + "getVirtualTrafficLight", + +[](lanelet::autoware::VirtualTrafficLight & self) { return self.getVirtualTrafficLight(); }) + .def( + "getStopLine", + +[](lanelet::autoware::VirtualTrafficLight & self) { return self.getStopLine(); }) + .def( + "getStartLine", + +[](lanelet::autoware::VirtualTrafficLight & self) { return self.getStartLine(); }) + .def( + "getEndLines", + +[](lanelet::autoware::VirtualTrafficLight & self) { return self.getEndLines(); }) + .def( + "__repr__", +[](lanelet::autoware::VirtualTrafficLight & r) { + return make_repr( + "VirtualTrafficLight", r.id(), repr(bp::dict(r.constData()->parameters)), + repr(r.attributes())); + }); + bp::implicitly_convertible< + std::shared_ptr, lanelet::RegulatoryElementPtr>(); +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp new file mode 100644 index 00000000..97af9689 --- /dev/null +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -0,0 +1,404 @@ +// Copyright 2023 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Mamoru Sobue + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace bp = boost::python; + +namespace +{ +// for handling functions with ros message type +lanelet::ArcCoordinates getArcCoordinates( + const lanelet::ConstLanelets & lanelet_sequence, 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); + return lanelet::utils::getArcCoordinates(lanelet_sequence, pose); +} + +double getLaneletAngle(const lanelet::ConstLanelet & lanelet, const std::string & point_byte) +{ + rclcpp::SerializedMessage serialized_msg; + static constexpr size_t message_header_length = 8u; + 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) { + serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + } + geometry_msgs::msg::Point point; + static rclcpp::Serialization serializer; + serializer.deserialize_message(&serialized_msg, &point); + return lanelet::utils::getLaneletAngle(lanelet, point); +} + +bool isInLanelet( + const std::string & pose_byte, const lanelet::ConstLanelet & lanelet, const double radius = 0.0) +{ + 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); + return lanelet::utils::isInLanelet(pose, lanelet, radius); +} + +std::vector getClosestCenterPose( + const lanelet::ConstLanelet & lanelet, const std::string & search_point_byte) +{ + rclcpp::SerializedMessage serialized_point_msg; + static constexpr size_t message_header_length = 8u; + 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) { + serialized_point_msg.get_rcl_serialized_message().buffer[i] = search_point_byte[i]; + } + 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; +} + +double getLateralDistanceToCenterline( + const lanelet::ConstLanelet & lanelet, 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); + return lanelet::utils::getLateralDistanceToCenterline(lanelet, pose); +} + +double getLateralDistanceToClosestLanelet( + const lanelet::ConstLanelets & lanelet_sequence, 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); + return lanelet::utils::getLateralDistanceToClosestLanelet(lanelet_sequence, pose); +} + +lanelet::ConstLanelets getLaneletsWithinRange_point( + const lanelet::ConstLanelets & lanelets, const std::string & point_byte, const double range) +{ + rclcpp::SerializedMessage serialized_msg; + static constexpr size_t message_header_length = 8u; + 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) { + serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + } + geometry_msgs::msg::Point point; + static rclcpp::Serialization serializer; + serializer.deserialize_message(&serialized_msg, &point); + return lanelet::utils::query::getLaneletsWithinRange(lanelets, point, range); +} + +lanelet::ConstLanelets getLaneChangeableNeighbors_point( + const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelets & road_lanelets, + const std::string & point_byte) +{ + rclcpp::SerializedMessage serialized_msg; + static constexpr size_t message_header_length = 8u; + 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) { + serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + } + geometry_msgs::msg::Point point; + static rclcpp::Serialization serializer; + serializer.deserialize_message(&serialized_msg, &point); + return lanelet::utils::query::getLaneChangeableNeighbors(graph, road_lanelets, point); +} + +lanelet::ConstLanelets getAllNeighbors_point( + const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelets & road_lanelets, + const std::string & point_byte) +{ + rclcpp::SerializedMessage serialized_msg; + static constexpr size_t message_header_length = 8u; + 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) { + serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + } + geometry_msgs::msg::Point point; + static rclcpp::Serialization serializer; + serializer.deserialize_message(&serialized_msg, &point); + return lanelet::utils::query::getAllNeighbors(graph, road_lanelets, point); +} + +bool 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()) +{ + 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); + return lanelet::utils::query::getClosestLaneletWithConstrains( + lanelets, pose, closest_lanelet_ptr, dist_threshold, yaw_threshold); +} + +bool getCurrentLanelets_point( + const lanelet::ConstLanelets & lanelets, const std::string & point_byte, + lanelet::ConstLanelets * current_lanelets_ptr) +{ + rclcpp::SerializedMessage serialized_msg; + static constexpr size_t message_header_length = 8u; + 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) { + serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i]; + } + 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); +} + +bool getCurrentLanelets_pose( + const lanelet::ConstLanelets & lanelets, const std::string & pose_byte, + lanelet::ConstLanelets * current_lanelets_ptr) +{ + 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); + return lanelet::utils::query::getCurrentLanelets(lanelets, pose, current_lanelets_ptr); +} +} // namespace + +// for handling functions with default arguments +BOOST_PYTHON_FUNCTION_OVERLOADS( + generateFineCenterline_overload, lanelet::utils::generateFineCenterline, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS( + getCenterlineWithOffset_overload, lanelet::utils::getCenterlineWithOffset, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS( + getRightBoundWithOffset_overload, lanelet::utils::getRightBoundWithOffset, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS( + getLeftBoundWithOffset_overload, lanelet::utils::getLeftBoundWithOffset, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS( + overwriteLaneletsCenterline_overload, lanelet::utils::overwriteLaneletsCenterline, 1, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(isInLanelet_overload, ::isInLanelet, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS( + getPrecedingLaneletSequences_overload, lanelet::utils::query::getPrecedingLaneletSequences, 3, 4) +BOOST_PYTHON_FUNCTION_OVERLOADS( + getClosestLaneletWithConstrains_overload, ::getClosestLaneletWithConstrains, 3, 5) + +BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) +{ + /* + converters::VectorToListConverter(); + converters::VectorToListConverter(); + converters::VectorToListConverter(); + */ + + // utilities.cpp + bp::def("combineLaneletsShape", lanelet::utils::combineLaneletsShape); + bp::def( + "generateFineCenterline", lanelet::utils::generateFineCenterline, + generateFineCenterline_overload()); + bp::def( + "getCenterlineWithOffset", lanelet::utils::getCenterlineWithOffset, + getCenterlineWithOffset_overload()); + bp::def( + "getRightBoundWithOffset", lanelet::utils::getRightBoundWithOffset, + getRightBoundWithOffset_overload()); + bp::def( + "getLeftBoundWithOffset", lanelet::utils::getLeftBoundWithOffset, + getLeftBoundWithOffset_overload()); + bp::def("getExpandedLanelet", lanelet::utils::getExpandedLanelet); + bp::def("getExpandedLanelets", lanelet::utils::getExpandedLanelets); + bp::def( + "overwriteLaneletsCenterline", lanelet::utils::overwriteLaneletsCenterline, + overwriteLaneletsCenterline_overload()); + bp::def( + "getLaneletLength2d", lanelet::utils::getLaneletLength2d); + bp::def( + "getLaneletLength3d", lanelet::utils::getLaneletLength3d); + bp::def( + "getLaneletLength2d", lanelet::utils::getLaneletLength2d); + bp::def( + "getLaneletLength3d", lanelet::utils::getLaneletLength3d); + bp::def("getArcCoordinates", ::getArcCoordinates); // depends ros msg + bp::def("getClosestSegment", lanelet::utils::getClosestSegment); + 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("getLateralDistanceToCenterline", ::getLateralDistanceToCenterline); // depends ros msg + bp::def( + "getLateralDistanceToClosestLanelet", ::getLateralDistanceToClosestLanelet); // depends ros msg + + // query.cpp + bp::def("laneletLayer", lanelet::utils::query::laneletLayer); + bp::def("subtypeLanelets", lanelet::utils::query::subtypeLanelets); + bp::def("crosswalkLanelets", lanelet::utils::query::crosswalkLanelets); + bp::def("walkwayLanelets", lanelet::utils::query::walkwayLanelets); + bp::def("roadLanelets", lanelet::utils::query::roadLanelets); + bp::def("shoulderLanelets", lanelet::utils::query::shoulderLanelets); + bp::def("trafficLights", lanelet::utils::query::trafficLights); + bp::def("autowareTrafficLights", lanelet::utils::query::autowareTrafficLights); + bp::def("detectionAreas", lanelet::utils::query::detectionAreas); + bp::def("noStoppingAreas", lanelet::utils::query::noStoppingAreas); + bp::def("noParkingAreas", lanelet::utils::query::noParkingAreas); + bp::def("speedBumps", lanelet::utils::query::speedBumps); + bp::def("crosswalks", lanelet::utils::query::crosswalks); + bp::def("curbstones", lanelet::utils::query::curbstones); + bp::def("getAllPolygonsByType", lanelet::utils::query::getAllPolygonsByType); + bp::def("getAllObstaclePolygons", lanelet::utils::query::getAllObstaclePolygons); + bp::def("getAllParkingLots", lanelet::utils::query::getAllParkingLots); + bp::def("getAllPartitions", lanelet::utils::query::getAllPartitions); + bp::def("getAllFences", lanelet::utils::query::getAllFences); + bp::def("getAllPedestrianMarkings", lanelet::utils::query::getAllPedestrianMarkings); + bp::def("getAllParkingSpaces", lanelet::utils::query::getAllParkingSpaces); + bp::def( + "getLinkedParkingSpaces", lanelet::utils::query::getLinkedParkingSpaces); + bp::def( + "getLinkedParkingSpaces", lanelet::utils::query::getLinkedParkingSpaces); + bp::def( + "getLinkedLanelet", lanelet::utils::query::getLinkedLanelet); + bp::def("getLinkedLanelet", lanelet::utils::query::getLinkedLanelet); + bp::def( + "getLinkedLanelets", lanelet::utils::query::getLinkedLanelets); + 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("stopLinesLanelets", lanelet::utils::query::stopLinesLanelets); + bp::def("stopLinesLanelet", lanelet::utils::query::stopLinesLanelet); + bp::def("stopSignStopLines", lanelet::utils::query::stopSignStopLines); + bp::def( + "getLaneletsWithinRange", lanelet::utils::query::getLaneletsWithinRange); + bp::def( + "getLaneletsWithinRange_point", ::getLaneletsWithinRange_point); // depends on ros msg + bp::def( + "getLaneChangeableNeighbors", lanelet::utils::query::getLaneChangeableNeighbors); + bp::def( + "getLaneChangeableNeighbors_point", ::getLaneChangeableNeighbors_point); // depends on ros msg + bp::def( + "getAllNeighbors", lanelet::utils::query::getAllNeighbors); + bp::def("getAllNeighbors_point", ::getAllNeighbors_point); // depends on ros msg + bp::def("getAllNeighborsLeft", lanelet::utils::query::getAllNeighborsLeft); + bp::def("getAllNeighborsRight", lanelet::utils::query::getAllNeighborsRight); + 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 + bp::def("getSucceedingLaneletSequences", lanelet::utils::query::getSucceedingLaneletSequences); + bp::def( + "getPrecedingLaneletSequences", lanelet::utils::query::getPrecedingLaneletSequences, + getPrecedingLaneletSequences_overload()); +} + +// NOLINTEND(readability-identifier-naming) From 543817e9c838e5acb4b11bd976e125c23da0c820 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 27 Dec 2023 15:42:04 +0900 Subject: [PATCH 2/9] conversion of regulatory elements seem to be duplicate Signed-off-by: Mamoru Sobue --- .../example/example.py | 127 +++++------------ .../regulatory_elements/__init__.py | 10 ++ .../utility/__init__.py | 1 + .../utility/query.py | 110 +++++++++++++++ .../utility/utilities.py | 128 ++---------------- .../src/projection.cpp | 7 - .../src/regulatory_elements.cpp | 9 ++ tmp/lanelet2_extension_python/src/utility.cpp | 28 +++- 8 files changed, 201 insertions(+), 219 deletions(-) create mode 100644 tmp/lanelet2_extension_python/lanelet2_extension_python/regulatory_elements/__init__.py create mode 100644 tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py diff --git a/tmp/lanelet2_extension_python/example/example.py b/tmp/lanelet2_extension_python/example/example.py index 7f300165..f8f0a415 100644 --- a/tmp/lanelet2_extension_python/example/example.py +++ b/tmp/lanelet2_extension_python/example/example.py @@ -1,110 +1,53 @@ -from geometry_msgs.msg import Pose import lanelet2 import lanelet2.geometry from lanelet2_extension_python.projection import MGRSProjector -from lanelet2_extension_python.utility.utilities import getArcCoordinates -from lanelet2_extension_python.utility.utilities import getLaneletAngle -from lanelet2_extension_python.utility.utilities import getLateralDistanceToClosestLanelet -from lanelet2_extension_python.utility.utilities import isInLanelet -import numpy as np +import lanelet2_extension_python.utility.query as query +# import lanelet2_extension_python.utility.utilities as utility -def print_layer(layer, layerName): - print("IDs in " + layerName) - print(sorted([elem.id for elem in layer])) +def test_projection(): + return MGRSProjector(lanelet2.io.Origin(0.0, 0.0)) -# Download the file from -# https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#preparation -if __name__ == "__main__": - proj = MGRSProjector(lanelet2.io.Origin(0.0, 0.0)) - ll2_map = lanelet2.io.load( - "/home/mamorusobue/workspace/pilot-auto.xx1/src/autoware/common/tmp/lanelet2_extension_python/example/sample-map-planning/lanelet2_map.osm", - proj, - ) - layers = { - # "Points": ll2_map.pointLayer, - # "Line Strings": ll2_map.lineStringLayer, - "Polygons": ll2_map.polygonLayer, - "Lanelets": ll2_map.laneletLayer, - "Areas": ll2_map.areaLayer, - # "Regulatory Elements": ll2_map.regulatoryElementLayer, - } +def test_io(map_path, projection): + return lanelet2.io.load(map_path, projection) - for layer_name, layer in layers.items(): - print_layer(layer, layer_name) - lanelet56 = ll2_map.laneletLayer.get(56) +def test_utility_query(lanelet_map, routing_graph): + lanelets = query.laneletLayer(lanelet_map) + print(f"{len(lanelets)=}") + print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""") + print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""") + print(f"""{len(query.crosswalkLanelets(lanelets))=}""") + print(f"""{len(query.walkwayLanelets(lanelets))=}""") + print(f"""{len(query.roadLanelets(lanelets))=}""") + print(f"""{len(query.shoulderLanelets(lanelets))=}""") + print(f"""{len(query.trafficLights(lanelets))=}""") + print(f"""{len(query.autowareTrafficLights(lanelets))=}""") + print(f"""{len(query.detectionAreas(lanelets))=}""") + print(f"""{len(query.noStoppingAreas(lanelets))=}""") + print(f"""{len(query.noParkingAreas(lanelets))=}""") + print(f"""{len(query.speedBumps(lanelets))=}""") + print(f"""{len(query.crosswalks(lanelets))=}""") - # regulatory element - # get associated traffic lights - lights = lanelet56.trafficLights() - for light in lights: - print(light.stopLine) - # get turn_direction - if "turn_direction" in lanelet56.attributes: - turn_direction = lanelet56.attributes["turn_direction"] - print(f"lanelet56 has {turn_direction} turn_direction value") - # routing - traffic_rules = lanelet2.traffic_rules.create( - lanelet2.traffic_rules.Locations.Germany, - lanelet2.traffic_rules.Participants.Vehicle, - ) - graph = lanelet2.routing.RoutingGraph(ll2_map, traffic_rules) - # get conflicting lanelets - conflictings = graph.conflicting(lanelet56) - print(f"lanelet56 is conflicting with {[conflicting.id for conflicting in conflictings]}") - # get following lanelets - followings = graph.following(lanelet56) - print(f"lanelet56 is connected to {[following.id for following in followings]}") - previouses = graph.previous(lanelet56) - print(f"lanelet56 is followed by {[previous.id for previous in previouses]}") +def test_utility_utilities(): + pass - centerline = lanelet56.centerline - print( - f"The centerline of lanelet56 is of id {centerline.id} and contains {len(centerline)} points." - ) - # access to points - pts = [] - for p in centerline: - pts.append(p) - # length, area - print(f"The length centerline is {lanelet2.geometry.length(centerline)}.") - basic_pts = [p.basicPoint() for p in pts] - center = np.sum(basic_pts) * (1.0 / len(basic_pts)) - - center_sd_frame = lanelet2.geometry.toArcCoordinates( - lanelet2.geometry.to2D(centerline), lanelet2.geometry.to2D(center) - ) - print( - f"""The center of centerline56 is ({center_sd_frame.distance}, {center_sd_frame.length}) in arc coordinates""" +if __name__ == "__main__": + proj = test_projection() + lanelet_map = test_io( + "/home/mamorusobue/workspace/pilot-auto.xx1/src/autoware/common/tmp/lanelet2_extension_python/example/sample-map-planning/lanelet2_map.osm", + proj, ) - # test utility - pose = Pose() - pose.position.x = center.x - pose.position.y = center.y - pose.position.z = center.z - print(f"pose = [{pose.position.x}, {pose.position.y}, {pose.position.z}]") - - arc_coord = getArcCoordinates([lanelet56], pose) - print(f"getArcCoordinates(lanelet56, pose) = {arc_coord.distance}, {arc_coord.length}") - - lanelet_angle = getLaneletAngle(lanelet56, pose.position) - print(lanelet_angle) - - print(f"isInLanelet(pose, lanelet56) = {isInLanelet(pose, lanelet56)}") - - """ - closest_center_pose = getClosestCenterPose(lanelet56, pose.position) - print( - f"getClosestCenterPose(lanelet56, pose.position) = ({closest_center_pose})" + traffic_rules = lanelet2.traffic_rules.create( + lanelet2.traffic_rules.Locations.Germany, + lanelet2.traffic_rules.Participants.Vehicle, ) - """ + routing_graph = lanelet2.routing.RoutingGraph(lanelet_map, traffic_rules) - print( - f"getLateralDistanceToClosestLanelet(lanelet56, pose) = {getLateralDistanceToClosestLanelet([lanelet56], pose)}" - ) + test_utility_utilities() + test_utility_query(lanelet_map, routing_graph) diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/regulatory_elements/__init__.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/regulatory_elements/__init__.py new file mode 100644 index 00000000..010ecc9c --- /dev/null +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/regulatory_elements/__init__.py @@ -0,0 +1,10 @@ +import lanelet2_extension_python._lanelet2_extension_python_boost_python_regulatory_elements as _regulatory_elements_cpp + +AutowareTrafficLight = _regulatory_elements_cpp.AutowareTrafficLight +Crosswalk = _regulatory_elements_cpp.Crosswalk +DetectionArea = _regulatory_elements_cpp.DetectionArea +NoParkingArea = _regulatory_elements_cpp.NoParkingArea +NoStoppingArea = _regulatory_elements_cpp.NoStoppingArea +RoadMarking = _regulatory_elements_cpp.RoadMarking +SpeedBump = _regulatory_elements_cpp.SpeedBump +VirtualTrafficLight = _regulatory_elements_cpp.VirtualTrafficLight diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py index e69de29b..5ba63f12 100644 --- a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py @@ -0,0 +1 @@ +import lanelet2_extension_python.regulatory_elements # noqa diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py new file mode 100644 index 00000000..6dc6fdca --- /dev/null +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py @@ -0,0 +1,110 @@ +from typing import overload + +from geometry_msgs.msg import Point +from geometry_msgs.msg import Pose +from lanelet2.core import BasicPoint2d +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 + +laneletLayer = _utility_cpp.laneletLayer +subtypeLanelets = _utility_cpp.subtypeLanelets +crosswalkLanelets = _utility_cpp.crosswalkLanelets +walkwayLanelets = _utility_cpp.walkwayLanelets +roadLanelets = _utility_cpp.roadLanelets +shoulderLanelets = _utility_cpp.shoulderLanelets +trafficLights = _utility_cpp.trafficLights +autowareTrafficLights = _utility_cpp.autowareTrafficLights +detectionAreas = _utility_cpp.detectionAreas +noStoppingAreas = _utility_cpp.noStoppingAreas +noParkingAreas = _utility_cpp.noParkingAreas +speedBumps = _utility_cpp.speedBumps +crosswalks = _utility_cpp.crosswalks +curbstones = _utility_cpp.curbstones +getAllPolygonsByType = _utility_cpp.getAllPolygonsByType +getAllObstaclePolygons = _utility_cpp.getAllObstaclePolygons +getAllParkingLots = _utility_cpp.getAllParkingLots +getAllPartitions = _utility_cpp.getAllPartitions +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 +# help(getLinkedLanelets) + +# TODO(Mamoru Sobue): how to dispatch overloads +getLinkedParkingLot = _utility_cpp.getLinkedParkingLot + +stopLinesLanelets = _utility_cpp.stopLinesLanelets +stopLinesLanelet = _utility_cpp.stopLinesLanelet +stopSignStopLines = _utility_cpp.stopSignStopLines + + +@overload +def getLaneletsWithinRange(lanelets, point: BasicPoint2d, rng: float): + return _utility_cpp.getLaneletsWithinRange(lanelets, point, rng) + + +@overload +def getLaneletsWithinRange(lanelets, point: Point, rng: float): + point_byte = serialize_message(point) + return _utility_cpp.getLaneletsWithinRange_point(lanelets, point_byte, rng) + + +@overload +def getLaneChangeableNeighbors(routing_graph, lanelet): + return _utility_cpp.getLaneChangeableNeighbor(routing_graph, lanelet) + + +@overload +def getLaneChangeableNeighbors(routing_graph, lanelets, point: Point): + point_byte = serialize_message(point) + return _utility_cpp.getLaneChangeableNeighbor_point(routing_graph, lanelets, point_byte) + + +@overload +def getAllNeighbors(routing_graph, lanelet): + return _utility_cpp.getAllNeighbors(routing_graph, lanelet) + + +@overload +def getAllNeighbors(routing_graph, lanelets, point: Point): + point_byte = serialize_message(point) + return _utility_cpp.getAllNeighbors(routing_graph, lanelets, point_byte) + + +getAllNeighborsLeft = _utility_cpp.getAllNeighborsLeft +getAllNeighborsRight = _utility_cpp.getAllNeighborsRight + + +def getClosestLaneletWithConstrains( + lanelets, pose: Pose, closest_lanelet, dist_threshold, yaw_threshold +): + pose_byte = serialize_message(pose) + return _utility_cpp.getClosestLaneletWithConstrains( + lanelets, pose_byte, closest_lanelet, dist_threshold, yaw_threshold + ) + + +@overload +def getCurrentLanelets(point: Point): + point_byte = serialize_message(point) + return _utility_cpp.getCurrentLanelets_point(point_byte) + + +@overload +def getCurrentLanelets(pose: Pose): + pose_byte = serialize_message(pose) + return _utility_cpp.getCurrentLanelets_pose(pose_byte) + + +getSucceedingLaneletSequences = _utility_cpp.getSucceedingLaneletSequences +getPrecedingLaneletSequences = _utility_cpp.getPrecedingLaneletSequences 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 1e0b6718..0dfb51e2 100644 --- a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py @@ -1,11 +1,7 @@ -from typing import overload - from geometry_msgs.msg import Point from geometry_msgs.msg import Pose -from lanelet2.core import BasicPoint2d import lanelet2_extension_python._lanelet2_extension_python_boost_python_utility as _utility_cpp - -# from rclpy.serialization import deserialize_message +from rclpy.serialization import deserialize_message from rclpy.serialization import serialize_message combineLaneletsShape = _utility_cpp.combineLaneletsShape @@ -39,17 +35,24 @@ def isInLanelet(pose: Pose, lanelet, radius=0.0): return _utility_cpp.isInLanelet(pose_byte, lanelet, radius) -""" -TODO 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) - bytes = [] + print(len(pose_byte)) # これは60ある + print(pose_byte[0].encode()) # これで b'\x00'と出た + byte_array = bytearray() for i in range(len(pose_byte)): - bytes.append(pose_byte[i].encode("latin1")) - print(bytes) - return deserialize_message(bytes, Pose) -""" + # 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) def getLateralDistanceToCenterline(lanelet, pose: Pose): @@ -60,104 +63,3 @@ def getLateralDistanceToCenterline(lanelet, pose: Pose): def getLateralDistanceToClosestLanelet(lanelet_sequence, pose: Pose): pose_byte = serialize_message(pose) return _utility_cpp.getLateralDistanceToClosestLanelet(lanelet_sequence, pose_byte) - - -laneletLayer = _utility_cpp.laneletLayer -subtypeLanelets = _utility_cpp.subtypeLanelets -crosswalkLanelets = _utility_cpp.crosswalkLanelets -walkwayLanelets = _utility_cpp.walkwayLanelets -roadLanelets = _utility_cpp.roadLanelets -shoulderLanelets = _utility_cpp.shoulderLanelets -trafficLights = _utility_cpp.trafficLights -autowareTrafficLights = _utility_cpp.autowareTrafficLights -noStoppingAreas = _utility_cpp.noStoppingAreas -noParkingAreas = _utility_cpp.noParkingAreas -speedBumps = _utility_cpp.speedBumps -crosswalks = _utility_cpp.crosswalks -curbstones = _utility_cpp.curbstones -getAllPolygonsByType = _utility_cpp.getAllPolygonsByType -getAllObstaclePolygons = _utility_cpp.getAllObstaclePolygons -getAllParkingLots = _utility_cpp.getAllParkingLots -getAllPartitions = _utility_cpp.getAllPartitions -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 -help(getLinkedLanelets) - -# TODO(Mamoru Sobue): how to dispatch overloads -getLinkedParkingLot = _utility_cpp.getLinkedParkingLot - -stopLinesLanelets = _utility_cpp.stopLinesLanelets -stopLinesLanelet = _utility_cpp.stopLinesLanelet -stopSignStopLines = _utility_cpp.stopSignStopLines - - -@overload -def getLaneletsWithinRange(lanelets, point: BasicPoint2d, rng: float): - return _utility_cpp.getLaneletsWithinRange(lanelets, point, rng) - - -@overload -def getLaneletsWithinRange(lanelets, point: Point, rng: float): - point_byte = serialize_message(point) - return _utility_cpp.getLaneletsWithinRange_point(lanelets, point_byte, rng) - - -@overload -def getLaneChangeableNeighbors(routing_graph, lanelet): - return _utility_cpp.getLaneChangeableNeighbor(routing_graph, lanelet) - - -@overload -def getLaneChangeableNeighbors(routing_graph, lanelets, point: Point): - point_byte = serialize_message(point) - return _utility_cpp.getLaneChangeableNeighbor_point(routing_graph, lanelets, point_byte) - - -@overload -def getAllNeighbors(routing_graph, lanelet): - return _utility_cpp.getAllNeighbors(routing_graph, lanelet) - - -@overload -def getAllNeighbors(routing_graph, lanelets, point: Point): - point_byte = serialize_message(point) - return _utility_cpp.getAllNeighbors(routing_graph, lanelets, point_byte) - - -getAllNeighborsLeft = _utility_cpp.getAllNeighborsLeft -getAllNeighborsRight = _utility_cpp.getAllNeighborsRight - - -def getClosestLaneletWithConstrains( - lanelets, pose: Pose, closest_lanelet, dist_threshold, yaw_threshold -): - pose_byte = serialize_message(pose) - return _utility_cpp.getClosestLaneletWithConstrains( - lanelets, pose_byte, closest_lanelet, dist_threshold, yaw_threshold - ) - - -@overload -def getCurrentLanelets(point: Point): - point_byte = serialize_message(point) - return _utility_cpp.getCurrentLanelets_point(point_byte) - - -@overload -def getCurrentLanelets(pose: Pose): - pose_byte = serialize_message(pose) - return _utility_cpp.getCurrentLanelets_pose(pose_byte) - - -getSucceedingLaneletSequences = _utility_cpp.getSucceedingLaneletSequences -getPrecedingLaneletSequences = _utility_cpp.getPrecedingLaneletSequences diff --git a/tmp/lanelet2_extension_python/src/projection.cpp b/tmp/lanelet2_extension_python/src/projection.cpp index 777ebe38..1f70a21d 100644 --- a/tmp/lanelet2_extension_python/src/projection.cpp +++ b/tmp/lanelet2_extension_python/src/projection.cpp @@ -29,13 +29,6 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_projection) { namespace bp = boost::python; - bp::class_>( - "Projector", "Projects point from lat/lon to x/y and back", bp::no_init) - .def("forward", &lanelet::Projector::forward, "Convert lat/lon into x/y") - .def("reverse", &lanelet::Projector::reverse, "Convert x/y into lat/lon") - .def( - "origin", &lanelet::Projector::origin, "Global origin of the converter", - bp::return_internal_reference<>()); bp::class_< lanelet::projection::MGRSProjector, std::shared_ptr, bp::bases>("MGRSProjector", bp::init("origin")); diff --git a/tmp/lanelet2_extension_python/src/regulatory_elements.cpp b/tmp/lanelet2_extension_python/src/regulatory_elements.cpp index c44af9c3..acddb819 100644 --- a/tmp/lanelet2_extension_python/src/regulatory_elements.cpp +++ b/tmp/lanelet2_extension_python/src/regulatory_elements.cpp @@ -293,6 +293,15 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_regulatory_elements) }); bp::implicitly_convertible< std::shared_ptr, lanelet::RegulatoryElementPtr>(); + + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); } // NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp index 97af9689..bed79bda 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -249,6 +249,13 @@ bool getCurrentLanelets_pose( 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()); +} + } // namespace // for handling functions with default arguments @@ -270,12 +277,6 @@ BOOST_PYTHON_FUNCTION_OVERLOADS( BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) { - /* - converters::VectorToListConverter(); - converters::VectorToListConverter(); - converters::VectorToListConverter(); - */ - // utilities.cpp bp::def("combineLaneletsShape", lanelet::utils::combineLaneletsShape); bp::def( @@ -315,8 +316,21 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) "getLateralDistanceToClosestLanelet", ::getLateralDistanceToClosestLanelet); // depends ros msg // query.cpp + /// AutowareTrafficLightConstPtr + converters::VectorToListConverter>(); + /// DetectionAreaConstPtr + converters::VectorToListConverter>(); + /// NoParkingAreaConstPtr + converters::VectorToListConverter>(); + /// NoStoppingAreaConstPtr + converters::VectorToListConverter>(); + /// SpeedBumpConstPtr + converters::VectorToListConverter>(); + /// CrosswalkConstPtr + converters::VectorToListConverter>(); + bp::def("laneletLayer", lanelet::utils::query::laneletLayer); - bp::def("subtypeLanelets", lanelet::utils::query::subtypeLanelets); + bp::def("subtypeLanelets", ::subtypeLanelets); bp::def("crosswalkLanelets", lanelet::utils::query::crosswalkLanelets); bp::def("walkwayLanelets", lanelet::utils::query::walkwayLanelets); bp::def("roadLanelets", lanelet::utils::query::roadLanelets); From 943cc42c46958cac95bec281ea37da10ed4d87e4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 27 Dec 2023 16:37:07 +0900 Subject: [PATCH 3/9] the overload dispatch of functions with same number arguments is WIP Signed-off-by: Mamoru Sobue --- .../example/example.py | 16 +++++++ tmp/lanelet2_extension_python/src/utility.cpp | 48 ++++++++++++------- 2 files changed, 46 insertions(+), 18 deletions(-) diff --git a/tmp/lanelet2_extension_python/example/example.py b/tmp/lanelet2_extension_python/example/example.py index f8f0a415..078df0bc 100644 --- a/tmp/lanelet2_extension_python/example/example.py +++ b/tmp/lanelet2_extension_python/example/example.py @@ -16,6 +16,8 @@ def test_io(map_path, projection): def test_utility_query(lanelet_map, routing_graph): lanelets = query.laneletLayer(lanelet_map) + lanelet56 = lanelet_map.laneletLayer.get(56) + lanelet108 = lanelet_map.laneletLayer.get(108) print(f"{len(lanelets)=}") print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""") print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""") @@ -30,6 +32,20 @@ def test_utility_query(lanelet_map, routing_graph): print(f"""{len(query.noParkingAreas(lanelets))=}""") print(f"""{len(query.speedBumps(lanelets))=}""") print(f"""{len(query.crosswalks(lanelets))=}""") + print(f"""{len(query.curbstones(lanelet_map))=}""") + print(f"""{len(query.getAllPolygonsByType(lanelet_map, "parking_lot"))=}""") + print(f"""{len(query.getAllParkingLots(lanelet_map))=}""") + print(f"""{len(query.getAllPartitions(lanelet_map))=}""") + 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))=}""") def test_utility_utilities(): diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp index bed79bda..9f66f65a 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -259,6 +259,15 @@ lanelet::ConstLanelets subtypeLanelets( } // 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) BOOST_PYTHON_FUNCTION_OVERLOADS( @@ -270,10 +279,6 @@ BOOST_PYTHON_FUNCTION_OVERLOADS( BOOST_PYTHON_FUNCTION_OVERLOADS( overwriteLaneletsCenterline_overload, lanelet::utils::overwriteLaneletsCenterline, 1, 3) BOOST_PYTHON_FUNCTION_OVERLOADS(isInLanelet_overload, ::isInLanelet, 2, 3) -BOOST_PYTHON_FUNCTION_OVERLOADS( - getPrecedingLaneletSequences_overload, lanelet::utils::query::getPrecedingLaneletSequences, 3, 4) -BOOST_PYTHON_FUNCTION_OVERLOADS( - getClosestLaneletWithConstrains_overload, ::getClosestLaneletWithConstrains, 3, 5) BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) { @@ -316,19 +321,6 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) "getLateralDistanceToClosestLanelet", ::getLateralDistanceToClosestLanelet); // depends ros msg // query.cpp - /// AutowareTrafficLightConstPtr - converters::VectorToListConverter>(); - /// DetectionAreaConstPtr - converters::VectorToListConverter>(); - /// NoParkingAreaConstPtr - converters::VectorToListConverter>(); - /// NoStoppingAreaConstPtr - converters::VectorToListConverter>(); - /// SpeedBumpConstPtr - converters::VectorToListConverter>(); - /// CrosswalkConstPtr - converters::VectorToListConverter>(); - bp::def("laneletLayer", lanelet::utils::query::laneletLayer); bp::def("subtypeLanelets", ::subtypeLanelets); bp::def("crosswalkLanelets", lanelet::utils::query::crosswalkLanelets); @@ -336,12 +328,25 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) bp::def("roadLanelets", lanelet::utils::query::roadLanelets); bp::def("shoulderLanelets", lanelet::utils::query::shoulderLanelets); bp::def("trafficLights", lanelet::utils::query::trafficLights); + bp::def("autowareTrafficLights", lanelet::utils::query::autowareTrafficLights); + converters::VectorToListConverter>(); + bp::def("detectionAreas", lanelet::utils::query::detectionAreas); + converters::VectorToListConverter>(); + bp::def("noStoppingAreas", lanelet::utils::query::noStoppingAreas); + converters::VectorToListConverter>(); + bp::def("noParkingAreas", lanelet::utils::query::noParkingAreas); + converters::VectorToListConverter>(); + bp::def("speedBumps", lanelet::utils::query::speedBumps); + converters::VectorToListConverter>(); + bp::def("crosswalks", lanelet::utils::query::crosswalks); + converters::VectorToListConverter>(); + bp::def("curbstones", lanelet::utils::query::curbstones); bp::def("getAllPolygonsByType", lanelet::utils::query::getAllPolygonsByType); bp::def("getAllObstaclePolygons", lanelet::utils::query::getAllObstaclePolygons); @@ -350,6 +355,7 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) bp::def("getAllFences", lanelet::utils::query::getAllFences); bp::def("getAllPedestrianMarkings", lanelet::utils::query::getAllPedestrianMarkings); bp::def("getAllParkingSpaces", lanelet::utils::query::getAllParkingSpaces); + bp::def( "getLinkedParkingSpaces", lanelet::utils::query::getLinkedParkingSpaces); @@ -357,6 +363,11 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) const lanelet::ConstLanelet &, const lanelet::ConstLineStrings3d &, const lanelet::ConstPolygons3d &)>( "getLinkedParkingSpaces", lanelet::utils::query::getLinkedParkingSpaces); + bp::class_("lanelet::ConstLineStrings3d") + .def(bp::vector_indexing_suite()); + bp::class_("lanelet::ConstPolygons3d") + .def(bp::vector_indexing_suite()); + bp::def( @@ -382,7 +393,8 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) lanelet::ConstPolygon3d *)>("getLinkedParkingLot", lanelet::utils::query::getLinkedParkingLot); bp::def("stopLinesLanelets", lanelet::utils::query::stopLinesLanelets); bp::def("stopLinesLanelet", lanelet::utils::query::stopLinesLanelet); - bp::def("stopSignStopLines", lanelet::utils::query::stopSignStopLines); + bp::def( + "stopSignStopLines", lanelet::utils::query::stopSignStopLines, stopSignStopLines_overload()); bp::def( "getLaneletsWithinRange", lanelet::utils::query::getLaneletsWithinRange); From 9a55d2213232f843a2f3f2b9f7198bd12b22cf2d Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 27 Dec 2023 18:41:57 +0900 Subject: [PATCH 4/9] getCurrentLanelets() is impossible Signed-off-by: Mamoru Sobue --- .../example/example.py | 38 +++++++++- .../utility/query.py | 70 ++++++++----------- tmp/lanelet2_extension_python/src/utility.cpp | 3 + 3 files changed, 68 insertions(+), 43 deletions(-) diff --git a/tmp/lanelet2_extension_python/example/example.py b/tmp/lanelet2_extension_python/example/example.py index 078df0bc..d8557fc4 100644 --- a/tmp/lanelet2_extension_python/example/example.py +++ b/tmp/lanelet2_extension_python/example/example.py @@ -1,9 +1,12 @@ +import math + +from geometry_msgs.msg import Point +from geometry_msgs.msg import Pose import lanelet2 import lanelet2.geometry from lanelet2_extension_python.projection import MGRSProjector import lanelet2_extension_python.utility.query as query - -# import lanelet2_extension_python.utility.utilities as utility +import numpy as np def test_projection(): @@ -46,6 +49,37 @@ def test_utility_query(lanelet_map, routing_graph): 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) + ) + 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"""{[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 + 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.getAllNeighbors(routing_graph, lanelet56)]=}""") + 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]}""") 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 6dc6fdca..70738c12 100644 --- a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py @@ -1,8 +1,6 @@ -from typing import overload - from geometry_msgs.msg import Point from geometry_msgs.msg import Pose -from lanelet2.core import BasicPoint2d +import lanelet2.core import lanelet2_extension_python._lanelet2_extension_python_boost_python_utility as _utility_cpp # from rclpy.serialization import deserialize_message @@ -38,7 +36,6 @@ # TODO(Mamoru Sobue): how to dispatch overloads getLinkedLanelets = _utility_cpp.getLinkedLanelets -# help(getLinkedLanelets) # TODO(Mamoru Sobue): how to dispatch overloads getLinkedParkingLot = _utility_cpp.getLinkedParkingLot @@ -48,37 +45,30 @@ stopSignStopLines = _utility_cpp.stopSignStopLines -@overload -def getLaneletsWithinRange(lanelets, point: BasicPoint2d, rng: float): - return _utility_cpp.getLaneletsWithinRange(lanelets, point, rng) - - -@overload -def getLaneletsWithinRange(lanelets, point: Point, rng: float): - point_byte = serialize_message(point) - return _utility_cpp.getLaneletsWithinRange_point(lanelets, point_byte, rng) - - -@overload -def getLaneChangeableNeighbors(routing_graph, lanelet): - return _utility_cpp.getLaneChangeableNeighbor(routing_graph, lanelet) +def getLaneletsWithinRange(lanelets, point, rng): + if isinstance(point, Point): + point_byte = serialize_message(point) + return _utility_cpp.getLaneletsWithinRange_point(lanelets, point_byte, rng) + if isinstance(point, lanelet2.core.BasicPoint2d): + return _utility_cpp.getLaneletsWithinRange(lanelets, point, rng) + raise TypeError("argument point is neither BasicPoint2d nor Point") -@overload -def getLaneChangeableNeighbors(routing_graph, lanelets, point: Point): - point_byte = serialize_message(point) - return _utility_cpp.getLaneChangeableNeighbor_point(routing_graph, lanelets, point_byte) +def getLaneChangeableNeighbors(*args): + if len(args) == 2 and isinstance(args[1], lanelet2.core.Lanelet): + 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) + raise TypeError("argument number does not match or 3rd argument is not Point type") -@overload -def getAllNeighbors(routing_graph, lanelet): - return _utility_cpp.getAllNeighbors(routing_graph, lanelet) - - -@overload -def getAllNeighbors(routing_graph, lanelets, point: Point): - point_byte = serialize_message(point) - return _utility_cpp.getAllNeighbors(routing_graph, lanelets, point_byte) +def getAllNeighbors(*args): + if len(args) == 2 and isinstance(args[1], lanelet2.core.Lanelet): + 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) getAllNeighborsLeft = _utility_cpp.getAllNeighborsLeft @@ -94,16 +84,14 @@ def getClosestLaneletWithConstrains( ) -@overload -def getCurrentLanelets(point: Point): - point_byte = serialize_message(point) - return _utility_cpp.getCurrentLanelets_point(point_byte) - - -@overload -def getCurrentLanelets(pose: Pose): - pose_byte = serialize_message(pose) - return _utility_cpp.getCurrentLanelets_pose(pose_byte) +def getCurrentLanelets(lanelets, point: Point, current_lanelets): + if isinstance(point, Point): + point_byte = serialize_message(point) + return _utility_cpp.getCurrentLanelets_point(lanelets, point_byte, current_lanelets) + 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") getSucceedingLaneletSequences = _utility_cpp.getSucceedingLaneletSequences diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp index 9f66f65a..ae2c1c02 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -363,6 +363,7 @@ 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 bp::class_("lanelet::ConstLineStrings3d") .def(bp::vector_indexing_suite()); bp::class_("lanelet::ConstPolygons3d") @@ -421,6 +422,8 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) 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 + bp::class_("lanelet::ConstLanelets") + .def(bp::vector_indexing_suite()); bp::def("getSucceedingLaneletSequences", lanelet::utils::query::getSucceedingLaneletSequences); bp::def( "getPrecedingLaneletSequences", lanelet::utils::query::getPrecedingLaneletSequences, From 76b0b52435f17b5c10ce86da15d22bdb53f018b7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 12 Jan 2024 17:36:16 +0900 Subject: [PATCH 5/9] remove pointer passing in lanelet2_extension Signed-off-by: Mamoru Sobue --- .../lanelet2_extension/utility/query.hpp | 42 ++-- .../lanelet2_extension/utility/utilities.hpp | 9 +- tmp/lanelet2_extension/lib/query.cpp | 202 +++++++----------- tmp/lanelet2_extension/lib/utilities.cpp | 50 ++--- tmp/lanelet2_extension/lib/visualization.cpp | 12 +- 5 files changed, 132 insertions(+), 183 deletions(-) diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp index 853d4a5d..e2fc6aba 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -164,13 +165,13 @@ lanelet::ConstLineStrings3d getLinkedParkingSpaces( const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, const lanelet::ConstPolygons3d & all_parking_lots); // query linked lanelets from parking space -bool getLinkedLanelet( +std::optional getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, - const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet); -bool getLinkedLanelet( + const lanelet::ConstPolygons3d & all_parking_lots); +std::optional getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, - const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet); + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); lanelet::ConstLanelets getLinkedLanelets( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, @@ -180,17 +181,16 @@ lanelet::ConstLanelets getLinkedLanelets( const lanelet::LaneletMapConstPtr & lanelet_map_ptr); // get linked parking lot from lanelet -bool getLinkedParkingLot( - const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, - lanelet::ConstPolygon3d * linked_parking_lot); +std::optional getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots); // get linked parking lot from current pose of ego car -bool getLinkedParkingLot( - const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, - lanelet::ConstPolygon3d * linked_parking_lot); +std::optional getLinkedParkingLot( + const lanelet::BasicPoint2d & current_position, + const lanelet::ConstPolygons3d & all_parking_lots); // get linked parking lot from parking space -bool getLinkedParkingLot( +std::optional getLinkedParkingLot( const lanelet::ConstLineString3d & parking_space, - const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot); + const lanelet::ConstPolygons3d & all_parking_lots); // query linked parking space from parking lot lanelet::ConstLineStrings3d getLinkedParkingSpaces( @@ -246,23 +246,19 @@ ConstLanelets getAllNeighbors( const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, const geometry_msgs::msg::Point & search_point); -bool getClosestLanelet( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelet * closest_lanelet_ptr); +std::optional getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose); -bool getClosestLaneletWithConstrains( +std::optional getClosestLaneletWithConstraints( const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelet * closest_lanelet_ptr, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -bool getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, - ConstLanelets * current_lanelets_ptr); +ConstLanelets getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point); -bool getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelets * current_lanelets_ptr); +ConstLanelets getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose); /** * [getSucceedingLaneletSequences retrieves a sequence of lanelets after the given lanelet. diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp index 7df3f6a2..e183d4cf 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp @@ -27,6 +27,7 @@ #include #include +#include namespace lanelet::utils { @@ -62,11 +63,11 @@ void overwriteLaneletsCenterline( lanelet::ConstLanelets getConflictingLanelets( const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet); -bool lineStringWithWidthToPolygon( - const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); +std::optional lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring); -bool lineStringToPolygon( - const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); +std::optional lineStringToPolygon( + const lanelet::ConstLineString3d & linestring); double getLaneletLength2d(const lanelet::ConstLanelet & lanelet); double getLaneletLength3d(const lanelet::ConstLanelet & lanelet); diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp index f444bdbd..5bd2c62f 100644 --- a/tmp/lanelet2_extension/lib/query.cpp +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -407,38 +407,38 @@ lanelet::ConstLineStrings3d query::getAllParkingSpaces( return parking_spaces; } -bool query::getLinkedLanelet( +std::optional query::getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, - const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet) + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr); const auto & all_road_lanelets = query::roadLanelets(all_lanelets); const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); - return query::getLinkedLanelet( - parking_space, all_road_lanelets, all_parking_lots, linked_lanelet); + return query::getLinkedLanelet(parking_space, all_road_lanelets, all_parking_lots); } -bool query::getLinkedLanelet( +std::optional query::getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, - const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet) + const lanelet::ConstPolygons3d & all_parking_lots) { const auto & linked_lanelets = getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots); if (linked_lanelets.empty()) { - return false; + return std::nullopt; } + std::optional min{std::nullopt}; double min_distance = std::numeric_limits::max(); for (const auto & lanelet : linked_lanelets) { const double distance = boost::geometry::distance( to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon()); if (distance < min_distance) { - *linked_lanelet = lanelet; + min = lanelet; min_distance = distance; } } - return true; + return min; } lanelet::ConstLanelets query::getLinkedLanelets( @@ -457,15 +457,15 @@ lanelet::ConstLanelets query::getLinkedLanelets( const lanelet::ConstLanelets & all_road_lanelets, const lanelet::ConstPolygons3d & all_parking_lots) { - lanelet::ConstLanelets linked_lanelets; - // get lanelets within same parking lot - lanelet::ConstPolygon3d linked_parking_lot; - if (!getLinkedParkingLot(parking_space, all_parking_lots, &linked_parking_lot)) { - return linked_lanelets; + const auto linked_parking_lot_opt = getLinkedParkingLot(parking_space, all_parking_lots); + if (!linked_parking_lot_opt) { + return {}; } + const auto linked_parking_lot = linked_parking_lot_opt.value(); const auto & candidate_lanelets = getLinkedLanelets(linked_parking_lot, all_road_lanelets); + lanelet::ConstLanelets linked_lanelets; // get lanelets that are close to parking space and facing to parking space for (const auto & lanelet : candidate_lanelets) { // check if parking space is close to lanelet @@ -522,16 +522,16 @@ lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, const lanelet::ConstPolygons3d & all_parking_lots) { - lanelet::ConstLineStrings3d linked_parking_spaces; - // get parking spaces that are in same parking lot. - lanelet::ConstPolygon3d linked_parking_lot; - if (!getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) { - return linked_parking_spaces; + const auto linked_parking_lot_opt = getLinkedParkingLot(lanelet, all_parking_lots); + if (!linked_parking_lot_opt) { + return {}; } + const auto linked_parking_lot = linked_parking_lot_opt.value(); const auto & possible_parking_spaces = getLinkedParkingSpaces(linked_parking_lot, all_parking_spaces); + lanelet::ConstLineStrings3d linked_parking_spaces; // check for parking spaces that are within 5m and facing towards lanelet for (const auto & parking_space : possible_parking_spaces) { // check if parking space is close to lanelet @@ -561,51 +561,46 @@ lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( } // get overlapping parking lot -bool query::getLinkedParkingLot( - const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, - lanelet::ConstPolygon3d * linked_parking_lot) +std::optional query::getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots) { for (const auto & parking_lot : all_parking_lots) { const double distance = boost::geometry::distance( lanelet.polygon2d().basicPolygon(), to2D(parking_lot).basicPolygon()); if (distance < std::numeric_limits::epsilon()) { - *linked_parking_lot = parking_lot; - return true; + return parking_lot; } } - return false; + return std::nullopt; } // get overlapping parking lot -bool query::getLinkedParkingLot( - const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, - lanelet::ConstPolygon3d * linked_parking_lot) +std::optional query::getLinkedParkingLot( + const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots) { for (const auto & parking_lot : all_parking_lots) { const double distance = boost::geometry::distance(current_position, to2D(parking_lot).basicPolygon()); if (distance < std::numeric_limits::epsilon()) { - *linked_parking_lot = parking_lot; - return true; + return parking_lot; } } - return false; + return std::nullopt; } // get overlapping parking lot -bool query::getLinkedParkingLot( +std::optional query::getLinkedParkingLot( const lanelet::ConstLineString3d & parking_space, - const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot) + const lanelet::ConstPolygons3d & all_parking_lots) { for (const auto & parking_lot : all_parking_lots) { const double distance = boost::geometry::distance( to2D(parking_space).basicLineString(), to2D(parking_lot).basicPolygon()); if (distance < std::numeric_limits::epsilon()) { - *linked_parking_lot = parking_lot; - return true; + return parking_lot; } } - return false; + return std::nullopt; } lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( @@ -827,84 +822,63 @@ ConstLanelets query::getAllNeighbors( return road_slices; } -bool query::getClosestLanelet( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelet * closest_lanelet_ptr) +std::optional query::getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose) { - if (closest_lanelet_ptr == nullptr) { - std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" - << std::endl; - return false; - } - if (lanelets.empty()) { - return false; + return std::nullopt; } - bool found = false; - lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); // find by distance lanelet::ConstLanelets candidate_lanelets; - { - double min_distance = std::numeric_limits::max(); - for (const auto & llt : lanelets) { - double distance = - boost::geometry::comparable_distance(llt.polygon2d().basicPolygon(), search_point); - - if (std::abs(distance - min_distance) <= std::numeric_limits::epsilon()) { - candidate_lanelets.push_back(llt); - } else if (distance < min_distance) { - found = true; - candidate_lanelets.clear(); - candidate_lanelets.push_back(llt); - min_distance = distance; - } + + std::optional closest_distance{std::nullopt}; + double min_distance = std::numeric_limits::max(); + for (const auto & llt : lanelets) { + double distance = + boost::geometry::comparable_distance(llt.polygon2d().basicPolygon(), search_point); + + if (std::abs(distance - min_distance) <= std::numeric_limits::epsilon()) { + candidate_lanelets.push_back(llt); + } else if (distance < min_distance) { + closest_distance = llt; + min_distance = distance; } } - if (candidate_lanelets.size() == 1) { - *closest_lanelet_ptr = candidate_lanelets.at(0); - return found; + if (closest_distance) { + return closest_distance.value(); } // find by angle - { - double min_angle = std::numeric_limits::max(); - double pose_yaw = tf2::getYaw(search_pose.orientation); - for (const auto & llt : candidate_lanelets) { - lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline()); - double angle_diff = M_PI; - if (!segment.empty()) { - double segment_angle = std::atan2( - segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); - angle_diff = std::abs(autoware_utils::normalize_radian(segment_angle - pose_yaw)); - } - if (angle_diff < min_angle) { - min_angle = angle_diff; - *closest_lanelet_ptr = llt; - } + std::optional closest_angle{std::nullopt}; + double min_angle = std::numeric_limits::max(); + double pose_yaw = tf2::getYaw(search_pose.orientation); + for (const auto & llt : candidate_lanelets) { + lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline()); + double angle_diff = M_PI; + if (!segment.empty()) { + double segment_angle = std::atan2( + segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); + angle_diff = std::abs(autoware_utils::normalize_radian(segment_angle - pose_yaw)); + } + if (angle_diff < min_angle) { + min_angle = angle_diff; + closest_angle = llt; } } - return found; + return closest_angle; } -bool query::getClosestLaneletWithConstrains( +std::optional query::getClosestLaneletWithConstraints( const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelet * closest_lanelet_ptr, const double dist_threshold, const double yaw_threshold) + const double dist_threshold, const double yaw_threshold) { - bool found = false; - - if (closest_lanelet_ptr == nullptr) { - std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" - << std::endl; - return found; - } - if (lanelets.empty()) { - return found; + return std::nullopt; } lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); @@ -928,11 +902,12 @@ bool query::getClosestLaneletWithConstrains( const std::pair & x, std::pair & y) { return x.second < y.second; }); } else { - return found; + return std::nullopt; } } // find closest lanelet within yaw_threshold + std::optional closest; { double min_angle = std::numeric_limits::max(); double min_distance = std::numeric_limits::max(); @@ -949,61 +924,48 @@ bool query::getClosestLaneletWithConstrains( if (angle_diff < min_angle) { min_angle = angle_diff; min_distance = distance; - *closest_lanelet_ptr = llt_pair.first; - found = true; + closest = llt_pair.first; } } } - return found; + return closest; } -bool query::getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, - ConstLanelets * current_lanelets_ptr) +ConstLanelets query::getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point) { - if (current_lanelets_ptr == nullptr) { - std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" - << std::endl; - return false; - } - if (lanelets.empty()) { - return false; + return {}; } + ConstLanelets current_lanelets; lanelet::BasicPoint2d search_point_2d(search_point.x, search_point.y); for (const auto & llt : lanelets) { if (lanelet::geometry::inside(llt, search_point_2d)) { - current_lanelets_ptr->push_back(llt); + current_lanelets.push_back(llt); } } - return !current_lanelets_ptr->empty(); // return found + return current_lanelets; } -bool query::getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelets * current_lanelets_ptr) +ConstLanelets query::getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose) { - if (current_lanelets_ptr == nullptr) { - std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" - << std::endl; - return false; - } - if (lanelets.empty()) { - return false; + return {}; } + ConstLanelets current_lanelets; lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); for (const auto & llt : lanelets) { if (lanelet::geometry::inside(llt, search_point)) { - current_lanelets_ptr->push_back(llt); + current_lanelets.push_back(llt); } } - return !current_lanelets_ptr->empty(); // return found + return current_lanelets; } std::vector> getSucceedingLaneletSequencesRecursive( diff --git a/tmp/lanelet2_extension/lib/utilities.cpp b/tmp/lanelet2_extension/lib/utilities.cpp index f84e9a43..c8c15b4c 100644 --- a/tmp/lanelet2_extension/lib/utilities.cpp +++ b/tmp/lanelet2_extension/lib/utilities.cpp @@ -523,24 +523,19 @@ lanelet::ConstLanelets getConflictingLanelets( return lanelets; } -bool lineStringWithWidthToPolygon( - const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) +std::optional lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring) { - if (polygon == nullptr) { - std::cerr << __func__ << ": polygon is null pointer! Failed to convert to polygon." - << std::endl; - return false; - } if (linestring.size() != 2) { std::cerr << __func__ << ": linestring" << linestring.id() << " must have 2 points! (" << linestring.size() << " != 2)" << std::endl << "Failed to convert to polygon."; - return false; + return std::nullopt; } if (!linestring.hasAttribute("width")) { std::cerr << __func__ << ": linestring" << linestring.id() << " does not have width tag. Failed to convert to polygon."; - return false; + return std::nullopt; } const Eigen::Vector3d direction = @@ -559,20 +554,12 @@ bool lineStringWithWidthToPolygon( const lanelet::Point3d p3(lanelet::InvalId, eigen_p3.x(), eigen_p3.y(), eigen_p3.z()); const lanelet::Point3d p4(lanelet::InvalId, eigen_p4.x(), eigen_p4.y(), eigen_p4.z()); - *polygon = lanelet::Polygon3d(lanelet::InvalId, {p1, p2, p3, p4}); - - return true; + return lanelet::Polygon3d(lanelet::InvalId, {p1, p2, p3, p4}); } -bool lineStringToPolygon( - const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) +std::optional lineStringToPolygon( + const lanelet::ConstLineString3d & linestring) { - if (polygon == nullptr) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("lanelet2_extension.visualization"), - __func__ << ": polygon is null pointer! Failed to convert to polygon."); - return false; - } if (linestring.size() < 4) { if (linestring.size() < 3 || linestring.front().id() == linestring.back().id()) { RCLCPP_WARN_STREAM( @@ -580,7 +567,7 @@ bool lineStringToPolygon( __func__ << ": linestring" << linestring.id() << " must have more than different 3 points! (size is " << linestring.size() << "). Failed to convert to polygon."); - return false; + return std::nullopt; } } @@ -595,9 +582,7 @@ bool lineStringToPolygon( llt_poly.pop_back(); } - *polygon = llt_poly; - - return true; + return llt_poly; } double getLaneletLength2d(const lanelet::ConstLanelet & lanelet) @@ -632,11 +617,14 @@ double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence) lanelet::ArcCoordinates getArcCoordinates( const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose) { - lanelet::ConstLanelet closest_lanelet; - lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet); + const auto closest_lanelet_opt = lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose); + lanelet::ArcCoordinates arc_coordinates; + if (!closest_lanelet_opt) { + return arc_coordinates; + } + const auto closest_lanelet = closest_lanelet_opt.value(); double length = 0; - lanelet::ArcCoordinates arc_coordinates; for (const auto & llt : lanelet_sequence) { const auto & centerline_2d = lanelet::utils::to2D(llt.centerline()); if (llt == closest_lanelet) { @@ -783,9 +771,11 @@ double getLateralDistanceToCenterline( double getLateralDistanceToClosestLanelet( const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose) { - lanelet::ConstLanelet closest_lanelet; - lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet); - return getLateralDistanceToCenterline(closest_lanelet, pose); + const auto closest_lanelet_opt = lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose); + if (!closest_lanelet_opt) { + return 0.0; + } + return getLateralDistanceToCenterline(closest_lanelet_opt.value(), pose); } } // namespace lanelet::utils diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 95301cef..1513d796 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -932,9 +932,9 @@ visualization_msgs::msg::MarkerArray visualization::pedestrianMarkingsAsMarkerAr visualization_msgs::msg::Marker marker = createPolygonMarker("pedestrian_marking", c); for (const auto & linestring : pedestrian_markings) { - lanelet::ConstPolygon3d polygon; - if (utils::lineStringToPolygon(linestring, &polygon)) { - pushPolygonMarker(&marker, polygon, c); + const auto polygon_opt = utils::lineStringToPolygon(linestring); + if (polygon_opt) { + pushPolygonMarker(&marker, polygon_opt.value(), c); } else { RCLCPP_WARN_STREAM( rclcpp::get_logger("lanelet2_extension.visualization"), @@ -976,9 +976,9 @@ visualization_msgs::msg::MarkerArray visualization::parkingSpacesAsMarkerArray( visualization_msgs::msg::Marker marker = createPolygonMarker("parking_space", c); for (const auto & linestring : parking_spaces) { - lanelet::ConstPolygon3d polygon; - if (utils::lineStringWithWidthToPolygon(linestring, &polygon)) { - pushPolygonMarker(&marker, polygon, c); + const auto polygon_opt = utils::lineStringToPolygon(linestring); + if (polygon_opt) { + pushPolygonMarker(&marker, polygon_opt.value(), c); } else { std::cerr << "parking space " << linestring.id() << " failed conversion." << std::endl; } From 122a735d7bb92a85da681e35fe773db1236c54d7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 12 Jan 2024 21:41:37 +0900 Subject: [PATCH 6/9] Revert "remove pointer passing in lanelet2_extension" This reverts commit e4e39bd4eaf3ddc79c8bf26afc7e612cd32548c3. Signed-off-by: Mamoru Sobue --- .../lanelet2_extension/utility/query.hpp | 42 ++-- .../lanelet2_extension/utility/utilities.hpp | 9 +- tmp/lanelet2_extension/lib/query.cpp | 202 +++++++++++------- tmp/lanelet2_extension/lib/utilities.cpp | 50 +++-- tmp/lanelet2_extension/lib/visualization.cpp | 12 +- 5 files changed, 183 insertions(+), 132 deletions(-) diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp index e2fc6aba..853d4a5d 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -34,7 +34,6 @@ #include #include -#include #include #include @@ -165,13 +164,13 @@ lanelet::ConstLineStrings3d getLinkedParkingSpaces( const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, const lanelet::ConstPolygons3d & all_parking_lots); // query linked lanelets from parking space -std::optional getLinkedLanelet( +bool getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, - const lanelet::ConstPolygons3d & all_parking_lots); -std::optional getLinkedLanelet( + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet); +bool getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, - const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet); lanelet::ConstLanelets getLinkedLanelets( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, @@ -181,16 +180,17 @@ lanelet::ConstLanelets getLinkedLanelets( const lanelet::LaneletMapConstPtr & lanelet_map_ptr); // get linked parking lot from lanelet -std::optional getLinkedParkingLot( - const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots); +bool getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot); // get linked parking lot from current pose of ego car -std::optional getLinkedParkingLot( - const lanelet::BasicPoint2d & current_position, - const lanelet::ConstPolygons3d & all_parking_lots); +bool getLinkedParkingLot( + const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot); // get linked parking lot from parking space -std::optional getLinkedParkingLot( +bool getLinkedParkingLot( const lanelet::ConstLineString3d & parking_space, - const lanelet::ConstPolygons3d & all_parking_lots); + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot); // query linked parking space from parking lot lanelet::ConstLineStrings3d getLinkedParkingSpaces( @@ -246,19 +246,23 @@ ConstLanelets getAllNeighbors( const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, const geometry_msgs::msg::Point & search_point); -std::optional getClosestLanelet( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose); +bool getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelet * closest_lanelet_ptr); -std::optional getClosestLaneletWithConstraints( +bool getClosestLaneletWithConstrains( const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelet * closest_lanelet_ptr, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -ConstLanelets getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point); +bool getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, + ConstLanelets * current_lanelets_ptr); -ConstLanelets getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose); +bool getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelets * current_lanelets_ptr); /** * [getSucceedingLaneletSequences retrieves a sequence of lanelets after the given lanelet. diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp index e183d4cf..7df3f6a2 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp @@ -27,7 +27,6 @@ #include #include -#include namespace lanelet::utils { @@ -63,11 +62,11 @@ void overwriteLaneletsCenterline( lanelet::ConstLanelets getConflictingLanelets( const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet); -std::optional lineStringWithWidthToPolygon( - const lanelet::ConstLineString3d & linestring); +bool lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); -std::optional lineStringToPolygon( - const lanelet::ConstLineString3d & linestring); +bool lineStringToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); double getLaneletLength2d(const lanelet::ConstLanelet & lanelet); double getLaneletLength3d(const lanelet::ConstLanelet & lanelet); diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp index 5bd2c62f..f444bdbd 100644 --- a/tmp/lanelet2_extension/lib/query.cpp +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -407,38 +407,38 @@ lanelet::ConstLineStrings3d query::getAllParkingSpaces( return parking_spaces; } -std::optional query::getLinkedLanelet( +bool query::getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, - const lanelet::LaneletMapConstPtr & lanelet_map_ptr) + const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet) { const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr); const auto & all_road_lanelets = query::roadLanelets(all_lanelets); const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); - return query::getLinkedLanelet(parking_space, all_road_lanelets, all_parking_lots); + return query::getLinkedLanelet( + parking_space, all_road_lanelets, all_parking_lots, linked_lanelet); } -std::optional query::getLinkedLanelet( +bool query::getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, - const lanelet::ConstPolygons3d & all_parking_lots) + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet) { const auto & linked_lanelets = getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots); if (linked_lanelets.empty()) { - return std::nullopt; + return false; } - std::optional min{std::nullopt}; double min_distance = std::numeric_limits::max(); for (const auto & lanelet : linked_lanelets) { const double distance = boost::geometry::distance( to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon()); if (distance < min_distance) { - min = lanelet; + *linked_lanelet = lanelet; min_distance = distance; } } - return min; + return true; } lanelet::ConstLanelets query::getLinkedLanelets( @@ -457,15 +457,15 @@ lanelet::ConstLanelets query::getLinkedLanelets( const lanelet::ConstLanelets & all_road_lanelets, const lanelet::ConstPolygons3d & all_parking_lots) { + lanelet::ConstLanelets linked_lanelets; + // get lanelets within same parking lot - const auto linked_parking_lot_opt = getLinkedParkingLot(parking_space, all_parking_lots); - if (!linked_parking_lot_opt) { - return {}; + lanelet::ConstPolygon3d linked_parking_lot; + if (!getLinkedParkingLot(parking_space, all_parking_lots, &linked_parking_lot)) { + return linked_lanelets; } - const auto linked_parking_lot = linked_parking_lot_opt.value(); const auto & candidate_lanelets = getLinkedLanelets(linked_parking_lot, all_road_lanelets); - lanelet::ConstLanelets linked_lanelets; // get lanelets that are close to parking space and facing to parking space for (const auto & lanelet : candidate_lanelets) { // check if parking space is close to lanelet @@ -522,16 +522,16 @@ lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, const lanelet::ConstPolygons3d & all_parking_lots) { + lanelet::ConstLineStrings3d linked_parking_spaces; + // get parking spaces that are in same parking lot. - const auto linked_parking_lot_opt = getLinkedParkingLot(lanelet, all_parking_lots); - if (!linked_parking_lot_opt) { - return {}; + lanelet::ConstPolygon3d linked_parking_lot; + if (!getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) { + return linked_parking_spaces; } - const auto linked_parking_lot = linked_parking_lot_opt.value(); const auto & possible_parking_spaces = getLinkedParkingSpaces(linked_parking_lot, all_parking_spaces); - lanelet::ConstLineStrings3d linked_parking_spaces; // check for parking spaces that are within 5m and facing towards lanelet for (const auto & parking_space : possible_parking_spaces) { // check if parking space is close to lanelet @@ -561,46 +561,51 @@ lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( } // get overlapping parking lot -std::optional query::getLinkedParkingLot( - const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots) +bool query::getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot) { for (const auto & parking_lot : all_parking_lots) { const double distance = boost::geometry::distance( lanelet.polygon2d().basicPolygon(), to2D(parking_lot).basicPolygon()); if (distance < std::numeric_limits::epsilon()) { - return parking_lot; + *linked_parking_lot = parking_lot; + return true; } } - return std::nullopt; + return false; } // get overlapping parking lot -std::optional query::getLinkedParkingLot( - const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots) +bool query::getLinkedParkingLot( + const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot) { for (const auto & parking_lot : all_parking_lots) { const double distance = boost::geometry::distance(current_position, to2D(parking_lot).basicPolygon()); if (distance < std::numeric_limits::epsilon()) { - return parking_lot; + *linked_parking_lot = parking_lot; + return true; } } - return std::nullopt; + return false; } // get overlapping parking lot -std::optional query::getLinkedParkingLot( +bool query::getLinkedParkingLot( const lanelet::ConstLineString3d & parking_space, - const lanelet::ConstPolygons3d & all_parking_lots) + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot) { for (const auto & parking_lot : all_parking_lots) { const double distance = boost::geometry::distance( to2D(parking_space).basicLineString(), to2D(parking_lot).basicPolygon()); if (distance < std::numeric_limits::epsilon()) { - return parking_lot; + *linked_parking_lot = parking_lot; + return true; } } - return std::nullopt; + return false; } lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( @@ -822,63 +827,84 @@ ConstLanelets query::getAllNeighbors( return road_slices; } -std::optional query::getClosestLanelet( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose) +bool query::getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelet * closest_lanelet_ptr) { + if (closest_lanelet_ptr == nullptr) { + std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" + << std::endl; + return false; + } + if (lanelets.empty()) { - return std::nullopt; + return false; } + bool found = false; + lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); // find by distance lanelet::ConstLanelets candidate_lanelets; - - std::optional closest_distance{std::nullopt}; - double min_distance = std::numeric_limits::max(); - for (const auto & llt : lanelets) { - double distance = - boost::geometry::comparable_distance(llt.polygon2d().basicPolygon(), search_point); - - if (std::abs(distance - min_distance) <= std::numeric_limits::epsilon()) { - candidate_lanelets.push_back(llt); - } else if (distance < min_distance) { - closest_distance = llt; - min_distance = distance; + { + double min_distance = std::numeric_limits::max(); + for (const auto & llt : lanelets) { + double distance = + boost::geometry::comparable_distance(llt.polygon2d().basicPolygon(), search_point); + + if (std::abs(distance - min_distance) <= std::numeric_limits::epsilon()) { + candidate_lanelets.push_back(llt); + } else if (distance < min_distance) { + found = true; + candidate_lanelets.clear(); + candidate_lanelets.push_back(llt); + min_distance = distance; + } } } - if (closest_distance) { - return closest_distance.value(); + if (candidate_lanelets.size() == 1) { + *closest_lanelet_ptr = candidate_lanelets.at(0); + return found; } // find by angle - std::optional closest_angle{std::nullopt}; - double min_angle = std::numeric_limits::max(); - double pose_yaw = tf2::getYaw(search_pose.orientation); - for (const auto & llt : candidate_lanelets) { - lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline()); - double angle_diff = M_PI; - if (!segment.empty()) { - double segment_angle = std::atan2( - segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); - angle_diff = std::abs(autoware_utils::normalize_radian(segment_angle - pose_yaw)); - } - if (angle_diff < min_angle) { - min_angle = angle_diff; - closest_angle = llt; + { + double min_angle = std::numeric_limits::max(); + double pose_yaw = tf2::getYaw(search_pose.orientation); + for (const auto & llt : candidate_lanelets) { + lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline()); + double angle_diff = M_PI; + if (!segment.empty()) { + double segment_angle = std::atan2( + segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); + angle_diff = std::abs(autoware_utils::normalize_radian(segment_angle - pose_yaw)); + } + if (angle_diff < min_angle) { + min_angle = angle_diff; + *closest_lanelet_ptr = llt; + } } } - return closest_angle; + return found; } -std::optional query::getClosestLaneletWithConstraints( +bool query::getClosestLaneletWithConstrains( const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - const double dist_threshold, const double yaw_threshold) + ConstLanelet * closest_lanelet_ptr, const double dist_threshold, const double yaw_threshold) { + bool found = false; + + if (closest_lanelet_ptr == nullptr) { + std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" + << std::endl; + return found; + } + if (lanelets.empty()) { - return std::nullopt; + return found; } lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); @@ -902,12 +928,11 @@ std::optional query::getClosestLaneletWithConstraints( const std::pair & x, std::pair & y) { return x.second < y.second; }); } else { - return std::nullopt; + return found; } } // find closest lanelet within yaw_threshold - std::optional closest; { double min_angle = std::numeric_limits::max(); double min_distance = std::numeric_limits::max(); @@ -924,48 +949,61 @@ std::optional query::getClosestLaneletWithConstraints( if (angle_diff < min_angle) { min_angle = angle_diff; min_distance = distance; - closest = llt_pair.first; + *closest_lanelet_ptr = llt_pair.first; + found = true; } } } - return closest; + return found; } -ConstLanelets query::getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point) +bool query::getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, + ConstLanelets * current_lanelets_ptr) { + if (current_lanelets_ptr == nullptr) { + std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" + << std::endl; + return false; + } + if (lanelets.empty()) { - return {}; + return false; } - ConstLanelets current_lanelets; lanelet::BasicPoint2d search_point_2d(search_point.x, search_point.y); for (const auto & llt : lanelets) { if (lanelet::geometry::inside(llt, search_point_2d)) { - current_lanelets.push_back(llt); + current_lanelets_ptr->push_back(llt); } } - return current_lanelets; + return !current_lanelets_ptr->empty(); // return found } -ConstLanelets query::getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose) +bool query::getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelets * current_lanelets_ptr) { + if (current_lanelets_ptr == nullptr) { + std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" + << std::endl; + return false; + } + if (lanelets.empty()) { - return {}; + return false; } - ConstLanelets current_lanelets; lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); for (const auto & llt : lanelets) { if (lanelet::geometry::inside(llt, search_point)) { - current_lanelets.push_back(llt); + current_lanelets_ptr->push_back(llt); } } - return current_lanelets; + return !current_lanelets_ptr->empty(); // return found } std::vector> getSucceedingLaneletSequencesRecursive( diff --git a/tmp/lanelet2_extension/lib/utilities.cpp b/tmp/lanelet2_extension/lib/utilities.cpp index c8c15b4c..f84e9a43 100644 --- a/tmp/lanelet2_extension/lib/utilities.cpp +++ b/tmp/lanelet2_extension/lib/utilities.cpp @@ -523,19 +523,24 @@ lanelet::ConstLanelets getConflictingLanelets( return lanelets; } -std::optional lineStringWithWidthToPolygon( - const lanelet::ConstLineString3d & linestring) +bool lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) { + if (polygon == nullptr) { + std::cerr << __func__ << ": polygon is null pointer! Failed to convert to polygon." + << std::endl; + return false; + } if (linestring.size() != 2) { std::cerr << __func__ << ": linestring" << linestring.id() << " must have 2 points! (" << linestring.size() << " != 2)" << std::endl << "Failed to convert to polygon."; - return std::nullopt; + return false; } if (!linestring.hasAttribute("width")) { std::cerr << __func__ << ": linestring" << linestring.id() << " does not have width tag. Failed to convert to polygon."; - return std::nullopt; + return false; } const Eigen::Vector3d direction = @@ -554,12 +559,20 @@ std::optional lineStringWithWidthToPolygon( const lanelet::Point3d p3(lanelet::InvalId, eigen_p3.x(), eigen_p3.y(), eigen_p3.z()); const lanelet::Point3d p4(lanelet::InvalId, eigen_p4.x(), eigen_p4.y(), eigen_p4.z()); - return lanelet::Polygon3d(lanelet::InvalId, {p1, p2, p3, p4}); + *polygon = lanelet::Polygon3d(lanelet::InvalId, {p1, p2, p3, p4}); + + return true; } -std::optional lineStringToPolygon( - const lanelet::ConstLineString3d & linestring) +bool lineStringToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) { + if (polygon == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __func__ << ": polygon is null pointer! Failed to convert to polygon."); + return false; + } if (linestring.size() < 4) { if (linestring.size() < 3 || linestring.front().id() == linestring.back().id()) { RCLCPP_WARN_STREAM( @@ -567,7 +580,7 @@ std::optional lineStringToPolygon( __func__ << ": linestring" << linestring.id() << " must have more than different 3 points! (size is " << linestring.size() << "). Failed to convert to polygon."); - return std::nullopt; + return false; } } @@ -582,7 +595,9 @@ std::optional lineStringToPolygon( llt_poly.pop_back(); } - return llt_poly; + *polygon = llt_poly; + + return true; } double getLaneletLength2d(const lanelet::ConstLanelet & lanelet) @@ -617,14 +632,11 @@ double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence) lanelet::ArcCoordinates getArcCoordinates( const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose) { - const auto closest_lanelet_opt = lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose); - lanelet::ArcCoordinates arc_coordinates; - if (!closest_lanelet_opt) { - return arc_coordinates; - } + lanelet::ConstLanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet); - const auto closest_lanelet = closest_lanelet_opt.value(); double length = 0; + lanelet::ArcCoordinates arc_coordinates; for (const auto & llt : lanelet_sequence) { const auto & centerline_2d = lanelet::utils::to2D(llt.centerline()); if (llt == closest_lanelet) { @@ -771,11 +783,9 @@ double getLateralDistanceToCenterline( double getLateralDistanceToClosestLanelet( const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose) { - const auto closest_lanelet_opt = lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose); - if (!closest_lanelet_opt) { - return 0.0; - } - return getLateralDistanceToCenterline(closest_lanelet_opt.value(), pose); + lanelet::ConstLanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet); + return getLateralDistanceToCenterline(closest_lanelet, pose); } } // namespace lanelet::utils diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 1513d796..95301cef 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -932,9 +932,9 @@ visualization_msgs::msg::MarkerArray visualization::pedestrianMarkingsAsMarkerAr visualization_msgs::msg::Marker marker = createPolygonMarker("pedestrian_marking", c); for (const auto & linestring : pedestrian_markings) { - const auto polygon_opt = utils::lineStringToPolygon(linestring); - if (polygon_opt) { - pushPolygonMarker(&marker, polygon_opt.value(), c); + lanelet::ConstPolygon3d polygon; + if (utils::lineStringToPolygon(linestring, &polygon)) { + pushPolygonMarker(&marker, polygon, c); } else { RCLCPP_WARN_STREAM( rclcpp::get_logger("lanelet2_extension.visualization"), @@ -976,9 +976,9 @@ visualization_msgs::msg::MarkerArray visualization::parkingSpacesAsMarkerArray( visualization_msgs::msg::Marker marker = createPolygonMarker("parking_space", c); for (const auto & linestring : parking_spaces) { - const auto polygon_opt = utils::lineStringToPolygon(linestring); - if (polygon_opt) { - pushPolygonMarker(&marker, polygon_opt.value(), c); + lanelet::ConstPolygon3d polygon; + if (utils::lineStringWithWidthToPolygon(linestring, &polygon)) { + pushPolygonMarker(&marker, polygon, c); } else { std::cerr << "parking space " << linestring.id() << " failed conversion." << std::endl; } From 08891b76047506ce01c280147948bbe0dc55080d Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 12 Jan 2024 22:26:07 +0900 Subject: [PATCH 7/9] finished except for getClosestCenterPose and query is tested completely. return vector for getClosestCenterPose at cpp Signed-off-by: Mamoru Sobue --- .../example/example.py | 53 ++-- .../utility/query.py | 26 +- .../utility/utilities.py | 47 ++-- tmp/lanelet2_extension_python/src/utility.cpp | 255 +++++++++++++----- 4 files changed, 259 insertions(+), 122 deletions(-) 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, From 8c2315d98e5223d741e2192fcd0acb52b69a7481 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 15 Jan 2024 16:51:48 +0900 Subject: [PATCH 8/9] adding test for utilities Signed-off-by: Mamoru Sobue --- tmp/lanelet2_extension_python/.gitignore | 1 - .../example/example.py | 114 ++++++++++++++++-- .../utility/utilities.py | 7 +- tmp/lanelet2_extension_python/src/utility.cpp | 6 +- 4 files changed, 108 insertions(+), 20 deletions(-) delete mode 100644 tmp/lanelet2_extension_python/.gitignore diff --git a/tmp/lanelet2_extension_python/.gitignore b/tmp/lanelet2_extension_python/.gitignore deleted file mode 100644 index 8ba839c5..00000000 --- a/tmp/lanelet2_extension_python/.gitignore +++ /dev/null @@ -1 +0,0 @@ -example/sample-map-planning diff --git a/tmp/lanelet2_extension_python/example/example.py b/tmp/lanelet2_extension_python/example/example.py index 5db2cf4c..2ba57f47 100644 --- a/tmp/lanelet2_extension_python/example/example.py +++ b/tmp/lanelet2_extension_python/example/example.py @@ -4,6 +4,8 @@ import lanelet2.geometry from lanelet2_extension_python.projection import MGRSProjector import lanelet2_extension_python.utility.query as query +import lanelet2_extension_python.utility.utilities as utilities +import matplotlib.pyplot as plt import numpy as np @@ -15,11 +17,104 @@ def test_io(map_path, projection): return lanelet2.io.load(map_path, projection) +def plot_ll2_id(ll2, ax, text): + xs, ys = np.array([pt.x for pt in ll2.centerline]), np.array([pt.y for pt in ll2.centerline]) + x, y = np.average(xs), np.average(ys) + ax.text(x, y, text) + + +def plot_linestring(linestring, ax, color, linestyle, label, **kwargs): + xs = [pt.x for pt in linestring] + ys = [pt.y for pt in linestring] + ax.plot(xs, ys, color=color, linestyle=linestyle, label=label, **kwargs) + + +def test_utility_utilities(lanelet_map, routing_graph): + lanelet108 = lanelet_map.laneletLayer.get(108) + lanelet108_next = query.getSucceedingLaneletSequences(routing_graph, lanelet108, 100.0) + lanelet108_seq = [lanelet108, *lanelet108_next[0]] + lanelet108_seq_combined = utilities.combineLaneletsShape(lanelet108_seq) + lanelet108_seq_combined_fine_centerline = utilities.generateFineCenterline( + lanelet108_seq_combined, 1.0 + ) + lanelet108_seq_combined_right_offset = utilities.getRightBoundWithOffset( + lanelet108_seq_combined, 1.0, 1.0 + ) + lanelet108_seq_combined_left_offset = utilities.getLeftBoundWithOffset( + lanelet108_seq_combined, 1.0, 1.0 + ) + fig = plt.figure() + ax = fig.add_subplot(1, 3, 1) + ax.axis("equal") + plot_linestring(lanelet108_seq_combined.leftBound, ax, "orange", "-", "108 left") + plot_linestring(lanelet108_seq_combined.rightBound, ax, "orange", "-", "108 right") + plot_linestring(lanelet108_seq_combined_fine_centerline, ax, "orange", "--", "108 center") + plot_linestring(lanelet108_seq_combined_right_offset, ax, "cyan", "--", "108 right offset") + plot_linestring(lanelet108_seq_combined_left_offset, ax, "red", "--", "108 left offset") + [plot_ll2_id(ll2, ax, str(ll2.id)) for ll2 in lanelet108_seq] + ax.set_title("test of combineLaneletsShape ~ getLeftBoundWithOffset") + + expanded_lanelet108 = utilities.getExpandedLanelet(lanelet108, 1.0, -1.0) + expanded_lanelet108_seq = utilities.getExpandedLanelets(lanelet108_seq, 2.0, -2.0) + ax = fig.add_subplot(1, 3, 2) + ax.axis("equal") + plot_linestring(expanded_lanelet108.leftBound, ax, "orange", "-", "expanded 108 left(1.0)") + plot_linestring(expanded_lanelet108.rightBound, ax, "orange", "-", "expanded 108 right(-1.0)") + plot_linestring(expanded_lanelet108.centerline, ax, "orange", "--", "expanded 108 center") + [ + ( + plot_linestring(ll2.leftBound, ax, "orange", "-", None), + plot_linestring(ll2.rightBound, ax, "orange", "-", None), + ) + for ll2 in expanded_lanelet108_seq + ] + [plot_ll2_id(ll2, ax, f"{ll2.id}(2.0)") for ll2 in expanded_lanelet108_seq] + ax.set_title("test of getExpandedLanelet(s)") + + print(f"""{utilities.getLaneletLength2d(lanelet108)=}""") + print(f"""{utilities.getLaneletLength2d(lanelet108_seq)=}""") + print(f"""{utilities.getLaneletLength3d(lanelet108)=}""") + print(f"""{utilities.getLaneletLength3d(lanelet108_seq)=}""") + + search_pose = Pose(position=Point(x=3685.0, y=73750.0)) + arc_coords = utilities.getArcCoordinates(lanelet108_seq, search_pose) + print( + f"""utilities.getArcCoordinates(lanelet108_seq, search_pose) = (length: {arc_coords.length}, distance: {arc_coords.distance})""" + ) + closest_lanelet = query.getClosestLanelet(lanelet108_seq, search_pose) + assert closest_lanelet.id == 156 + closest_segment = utilities.getClosestSegment( + lanelet2.core.BasicPoint2d(x=search_pose.position.x, y=search_pose.position.y), + closest_lanelet.centerline, + ) + ax = fig.add_subplot(1, 3, 3) + ax.axis("equal") + plot_linestring(closest_lanelet.leftBound, ax, "orange", "--", "156 left") + plot_linestring(closest_lanelet.rightBound, ax, "orange", "--", "156 right") + plot_linestring(closest_lanelet.centerline, ax, "cyan", "--", "156 center") + plot_linestring(closest_segment, ax, "red", "-", "closest segment", linewidth=2) + ax.scatter([search_pose.position.x], [search_pose.position.y], marker="o", label="search_pose") + + print(f"""{utilities.getLaneletAngle(closest_lanelet, search_pose.position)=}""") + print(f"""{utilities.isInLanelet(search_pose, closest_lanelet)=}""") + print(f"""{utilities.isInLanelet(search_pose, closest_lanelet, 5.0)=}""") + closest_center_pose = utilities.getClosestCenterPose(closest_lanelet, search_pose.position) + ax.scatter( + [closest_center_pose.position.x], + [closest_center_pose.position.y], + marker="o", + label="closest_center_pose", + ) + print(f"""{utilities.getLateralDistanceToCenterline(closest_lanelet, search_pose)=}""") + print(f"""{utilities.getLateralDistanceToClosestLanelet(lanelet108_seq, search_pose)=}""") + plt.legend() + plt.show() + + def test_utility_query(lanelet_map, routing_graph): lanelets = query.laneletLayer(lanelet_map) lanelet56 = lanelet_map.laneletLayer.get(56) lanelet108 = lanelet_map.laneletLayer.get(108) - print(f"{len(lanelets)=}") print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""") print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""") print(f"""{len(query.crosswalkLanelets(lanelets))=}""") @@ -76,23 +171,18 @@ def test_utility_query(lanelet_map, routing_graph): ) 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)]}""" + 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)]}""" + f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getPrecedingLaneletSequences(routing_graph, lanelet108, 100.0)]=}""" ) -def test_utility_utilities(): - pass - - if __name__ == "__main__": proj = test_projection() - lanelet_map = test_io( - "/home/mamorusobue/workspace/pilot-auto.xx1/src/autoware/common/tmp/lanelet2_extension_python/example/sample-map-planning/lanelet2_map.osm", - proj, - ) + # https://docs.google.com/uc?export=download&id=1499_nsbUbIeturZaDj7jhUownh5fvXHd. + # See https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/ + lanelet_map = test_io("", proj) traffic_rules = lanelet2.traffic_rules.create( lanelet2.traffic_rules.Locations.Germany, @@ -100,5 +190,5 @@ def test_utility_utilities(): ) routing_graph = lanelet2.routing.RoutingGraph(lanelet_map, traffic_rules) - test_utility_utilities() + test_utility_utilities(lanelet_map, routing_graph) test_utility_query(lanelet_map, routing_graph) 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 12031dc8..27acf993 100644 --- a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py @@ -14,8 +14,8 @@ getExpandedLanelets = _utility_cpp.getExpandedLanelets overwriteLaneletsCenterline = _utility_cpp.overwriteLaneletsCenterline getConflictingLanelets = _utility_cpp.getConflictingLanelets -lineStringWithWidthToPolygon = _utility_cpp._utility_cpp.lineStringWithWidthToPolygon -lineStringToPolygon = _utility_cpp._utility_cpp.lineStringToPolygon +lineStringWithWidthToPolygon = _utility_cpp.lineStringWithWidthToPolygon +lineStringToPolygon = _utility_cpp.lineStringToPolygon def getLaneletLength2d(*args): @@ -58,12 +58,11 @@ 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_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) + return Pose(position=pos, orientation=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 44dedad9..fa69bc34 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -122,7 +122,7 @@ std::vector getClosestCenterPose( static rclcpp::Serialization serializer_point; serializer_point.deserialize_message(&serialized_point_msg, &search_point); const geometry_msgs::msg::Pose pose = lanelet::utils::getClosestCenterPose(lanelet, search_point); - // NOTE: it was difficult to return the deserialized pose_byte and seriealize the pose_byte on + // NOTE: it was difficult to return the deserialized pose_byte and serialize 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; @@ -429,7 +429,7 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) bp::def("isInLanelet", ::isInLanelet, isInLanelet_overload()); // depends ros msg bp::def("getClosestCenterPose", ::getClosestCenterPose); // depends ros msg // NOTE: required for the return-value of getClosestCenterPose - bp::class_>("[position, quarternion]") + bp::class_>("[position, quaternion]") .def(bp::vector_indexing_suite>()); bp::def("getLateralDistanceToCenterline", ::getLateralDistanceToCenterline); // depends ros msg bp::def( @@ -550,7 +550,7 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) // 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 + // 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); From a7de2615fd75d72776177c3da083616855f3ec9a Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 15 Jan 2024 20:29:40 +0900 Subject: [PATCH 9/9] remove geometry Signed-off-by: Mamoru Sobue --- .../src/geometry.cpp | 35 ------------------- 1 file changed, 35 deletions(-) delete mode 100644 tmp/lanelet2_extension_python/src/geometry.cpp diff --git a/tmp/lanelet2_extension_python/src/geometry.cpp b/tmp/lanelet2_extension_python/src/geometry.cpp deleted file mode 100644 index f467446e..00000000 --- a/tmp/lanelet2_extension_python/src/geometry.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2023 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Authors: Mamoru Sobue - -// NOLINTBEGIN(readability-identifier-naming) - -#include -#include - -#include - -BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) -{ - namespace bp = boost::python; - // NOTE: should be added to Lanelet2 mainstream - // missing method of lanelet2_python/python_api/geometry.cpp - // def("area", boost::geometry::area); - bp::def("area", boost::geometry::area); - // def("area", boost::geometry::area); - bp::def("area", boost::geometry::area); -} - -// NOLINTEND(readability-identifier-naming)