From bf45338a8d5ef665dabbbe3a3d9f692ec3a86296 Mon Sep 17 00:00:00 2001 From: Johannes Fischer Date: Tue, 5 Nov 2024 21:40:05 +0100 Subject: [PATCH 1/4] Allow printing only ids of lanelet sequence --- lanelet2_python/python_api/core.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lanelet2_python/python_api/core.cpp b/lanelet2_python/python_api/core.cpp index 54faea85..639f23be 100644 --- a/lanelet2_python/python_api/core.cpp +++ b/lanelet2_python/python_api/core.cpp @@ -1035,6 +1035,8 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .def("__iter__", iterator()) .def("__len__", &LaneletSequence::size, "Number of lanelets in this sequence") .def("inverted", &LaneletSequence::inverted, "True if this lanelet sequence is inverted") + .def( + "__str__", +[](const LaneletSequence& s) { return makeRepr("LaneletSequence", s.lanelets()); }) .def( "__repr__", +[](const LaneletSequence& s) { return makeRepr("LaneletSequence", repr(object(s.lanelets()))); }) .def("__getitem__", wrappers::getItem, return_internal_reference<>(), From 2444710f7e4e80d442adb154b5575523c214ad1a Mon Sep 17 00:00:00 2001 From: Johannes Fischer Date: Tue, 5 Nov 2024 21:58:38 +0100 Subject: [PATCH 2/4] Fix RecursionError from repr of lanelet with regulatory elements --- .../primitives/RegulatoryElement.h | 2 + lanelet2_core/src/RegulatoryElement.cpp | 22 ++++++- lanelet2_python/python_api/core.cpp | 57 ++++++++++++++++--- 3 files changed, 70 insertions(+), 11 deletions(-) diff --git a/lanelet2_core/include/lanelet2_core/primitives/RegulatoryElement.h b/lanelet2_core/include/lanelet2_core/primitives/RegulatoryElement.h index 375e0202..d0747231 100644 --- a/lanelet2_core/include/lanelet2_core/primitives/RegulatoryElement.h +++ b/lanelet2_core/include/lanelet2_core/primitives/RegulatoryElement.h @@ -457,6 +457,8 @@ inline boost::optional RegulatoryElement::find(Id id) cons } std::ostream& operator<<(std::ostream& stream, const RegulatoryElement& obj); +std::ostream& operator<<(std::ostream& stream, const RuleParameterMap& map); +std::ostream& operator<<(std::ostream& stream, const ConstRuleParameterMap& map); template <> inline std::vector RegulatoryElement::getParameters(const std::string& role) const { diff --git a/lanelet2_core/src/RegulatoryElement.cpp b/lanelet2_core/src/RegulatoryElement.cpp index 37679656..5331115d 100644 --- a/lanelet2_core/src/RegulatoryElement.cpp +++ b/lanelet2_core/src/RegulatoryElement.cpp @@ -176,15 +176,31 @@ std::ostream& operator<<(std::ostream& stream, const RegulatoryElement& obj) { stream << "[id: " << obj.id(); if (!obj.empty()) { stream << ", parameters: "; - for (auto& param : obj.getParameters()) { + stream << obj.getParameters(); + } + return stream << ']'; +} + +std::ostream& operator<<(std::ostream& stream, const RuleParameterMap& map) { + for (const auto& param : map) { stream << '{' << param.first << ':' << ' '; for (auto& rule : param.second) { stream << GetIdVisitor::id(rule) << ' '; } stream << '}'; } - } - return stream << ']'; + return stream; +} + +std::ostream& operator<<(std::ostream& stream, const ConstRuleParameterMap& map) { + for (const auto& param : map) { + stream << '{' << param.first << ':' << ' '; + for (auto& rule : param.second) { + stream << GetIdVisitor::id(rule) << ' '; + } + stream << '}'; + } + return stream; } template <> diff --git a/lanelet2_python/python_api/core.cpp b/lanelet2_python/python_api/core.cpp index 639f23be..34d2c08a 100644 --- a/lanelet2_python/python_api/core.cpp +++ b/lanelet2_python/python_api/core.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -454,6 +455,11 @@ std::string repr(const object& o) { return call(repr.ptr(), o); } +std::string strRepr(const object& o) { + object str = import("builtins").attr("str"); + return call(str.ptr(), o); +} + std::string repr(const AttributeMap& a) { if (a.empty()) { return {}; @@ -461,6 +467,39 @@ std::string repr(const AttributeMap& a) { return repr(object(a)); } +std::string repr(const ConstRuleParameterMap& r) { + if (r.empty()) { + return {}; + } + return repr(object(r)); +} + +std::string safeRepr(const RuleParameter& p) { + if (p.type() == typeid(WeakLanelet)) { + // repr(lanelet) inside a traffic rule can result in a circular repr call between the lanelet and the traffic rule + // RecursionError: maximum recursion depth exceeded while calling a Python object + return "Lanelet(" + strRepr(object(p)) + ")"; + } + return repr(object(p)); +} + +std::string safeRepr(const ConstRuleParameter& p) { + if (p.type() == typeid(ConstWeakLanelet)) { + // repr(lanelet) inside a traffic rule can result in a circular repr call between the lanelet and the traffic rule + // RecursionError: maximum recursion depth exceeded while calling a Python object + return "Lanelet(" + strRepr(object(p)) + ")"; + } + return repr(object(p)); +} + +std::string repr(const RuleParameters& p) { + return "[" + boost::algorithm::join(utils::transform(p, [](const auto& elem) { return safeRepr(elem); }), ", ") + "]"; +} + +std::string repr(const ConstRuleParameters& p) { + return "[" + boost::algorithm::join(utils::transform(p, [](const auto& elem) { return safeRepr(elem); }), ", ") + "]"; +} + std::string repr(const RegulatoryElementConstPtrs& regelems) { if (regelems.empty()) { return {}; @@ -685,13 +724,15 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT "primitives with that role (parameters).", init<>("Create empty rule parameter map")) .def(IsHybridMap()) + .def(self_ns::str(self_ns::self)) .def( - "__repr__", +[](const RuleParameterMap& r) { return makeRepr("RuleParameterMap", repr(dict(r))); }); + "__repr__", +[](const RuleParameterMap& r) { return makeRepr("RuleParameterMap", "{" + boost::algorithm::join(utils::transform(r, [](const auto& elem) { return "'" + elem.first + "': " + repr((elem.second)); }), ", ") + "}"); }); class_("ConstRuleParameterMap", init<>("ConstRuleParameterMap()")) .def(IsHybridMap()) + .def(self_ns::str(self_ns::self)) .def( - "__repr__", +[](const RuleParameterMap& r) { return makeRepr("RuleParameterMap", repr(dict(r))); }); + "__repr__", +[](const ConstRuleParameterMap& r) { return makeRepr("RuleParameterMap", "{" + boost::algorithm::join(utils::transform(r, [](const auto& elem) { return "'" + elem.first + "': " + repr((elem.second)); }), ", ") + "}"); }); class_("ConstPoint2d", "Immutable 2D point primitive. It can not be instanciated from python but is returned from a " @@ -1181,7 +1222,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .def("__len__", &RegulatoryElement::size) .def( "__repr__", +[](RegulatoryElement& r) { - return makeRepr("RegulatoryElement", r.id(), repr(dict(r.constData()->parameters)), repr(r.attributes())); + return makeRepr("RegulatoryElement", r.id(), repr(r.getParameters()), repr(r.attributes())); }); register_ptr_to_python(); @@ -1204,7 +1245,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT "Removes a given traffic light, returns true on success") .def( "__repr__", +[](TrafficLight& r) { - return makeRepr("TrafficLight", r.id(), repr(dict(r.constData()->parameters)), repr(r.attributes())); + return makeRepr("TrafficLight", r.id(), repr(r.getParameters()), repr(r.attributes())); }); implicitly_convertible, RegulatoryElementPtr>(); @@ -1243,7 +1284,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT "Removes the yielding lanelet, returns true on success.") .def( "__repr__", +[](RightOfWay& r) { - return makeRepr("RightOfWay", r.id(), repr(dict(r.constData()->parameters)), repr(r.attributes())); + return makeRepr("RightOfWay", r.id(), repr(r.getParameters()), repr(r.attributes())); }); implicitly_convertible, RegulatoryElementPtr>(); @@ -1275,7 +1316,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT "Removes a lanelet from the all way stop, returns True on success.") .def( "__repr__", +[](AllWayStop& r) { - return makeRepr("AllWayStop", r.id(), repr(dict(r.constData()->parameters)), repr(r.attributes())); + return makeRepr("AllWayStop", r.id(), repr(r.getParameters()), repr(r.attributes())); }); implicitly_convertible, RegulatoryElementPtr>(); @@ -1337,7 +1378,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT "Returns types of the cancelling traffic signs (a list of strings) if it exists.") .def( "__repr__", +[](TrafficSign& r) { - return makeRepr("TrafficSign", r.id(), repr(dict(r.constData()->parameters)), repr(r.attributes())); + return makeRepr("TrafficSign", r.id(), repr(r.getParameters()), repr(r.attributes())); }); implicitly_convertible, RegulatoryElementPtr>(); @@ -1353,7 +1394,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT arg("refLines") = LineStrings3d(), arg("cancelLines") = LineStrings3d()))) .def( "__repr__", +[](SpeedLimit& r) { - return makeRepr("SpeedLimit", r.id(), repr(dict(r.constData()->parameters)), repr(r.attributes())); + return makeRepr("SpeedLimit", r.id(), repr(r.getParameters()), repr(r.attributes())); }); class_, boost::noncopyable>("PrimitiveLayerArea", no_init); // NOLINT From 7feb559f6816cbf7be4c741236b34b9f3c9eb7c6 Mon Sep 17 00:00:00 2001 From: Johannes Fischer Date: Fri, 22 Nov 2024 18:19:36 +0100 Subject: [PATCH 3/4] Replace safeRepr by RuleParameterReprVisitor --- lanelet2_python/python_api/core.cpp | 46 +++++++++++++++++++---------- 1 file changed, 31 insertions(+), 15 deletions(-) diff --git a/lanelet2_python/python_api/core.cpp b/lanelet2_python/python_api/core.cpp index 34d2c08a..c0eaea46 100644 --- a/lanelet2_python/python_api/core.cpp +++ b/lanelet2_python/python_api/core.cpp @@ -467,37 +467,53 @@ std::string repr(const AttributeMap& a) { return repr(object(a)); } -std::string repr(const ConstRuleParameterMap& r) { +std::string repr(const RuleParameterMap& r) { if (r.empty()) { return {}; } return repr(object(r)); } -std::string safeRepr(const RuleParameter& p) { - if (p.type() == typeid(WeakLanelet)) { - // repr(lanelet) inside a traffic rule can result in a circular repr call between the lanelet and the traffic rule - // RecursionError: maximum recursion depth exceeded while calling a Python object - return "Lanelet(" + strRepr(object(p)) + ")"; +std::string repr(const ConstRuleParameterMap& r) { + if (r.empty()) { + return {}; } - return repr(object(p)); + return repr(object(r)); } -std::string safeRepr(const ConstRuleParameter& p) { - if (p.type() == typeid(ConstWeakLanelet)) { - // repr(lanelet) inside a traffic rule can result in a circular repr call between the lanelet and the traffic rule + +struct RuleParameterReprVisitor : public RuleParameterVisitor { + std::string getRepr(const ConstRuleParameter& from) { + boost::apply_visitor(*this, from); + return this->repr_; + } + void operator()(const ConstPoint3d& p) override { repr_ = repr(object(p)); } + void operator()(const ConstLineString3d& ls) override { repr_ = repr(object(ls)); } + void operator()(const ConstPolygon3d& p) override { repr_ = repr(object(p)); } + void operator()(const ConstWeakLanelet& ll) override { + if (ll.expired()) { + return; + } + // print str representation to avoid circular dependency between lanelet and traffic rules, otherwise: // RecursionError: maximum recursion depth exceeded while calling a Python object - return "Lanelet(" + strRepr(object(p)) + ")"; + repr_ = strRepr(object(ll.lock())); } - return repr(object(p)); -} + void operator()(const ConstWeakArea& ar) override { + if (ar.expired()) { + return; + } + repr_ = strRepr(object(ar.lock())); + } + private: + std::string repr_{}; +}; std::string repr(const RuleParameters& p) { - return "[" + boost::algorithm::join(utils::transform(p, [](const auto& elem) { return safeRepr(elem); }), ", ") + "]"; + return "[" + boost::algorithm::join(utils::transform(p, [](const auto& elem) { return RuleParameterReprVisitor().getRepr(elem); }), ", ") + "]"; } std::string repr(const ConstRuleParameters& p) { - return "[" + boost::algorithm::join(utils::transform(p, [](const auto& elem) { return safeRepr(elem); }), ", ") + "]"; + return "[" + boost::algorithm::join(utils::transform(p, [](const auto& elem) { return RuleParameterReprVisitor().getRepr(elem); }), ", ") + "]"; } std::string repr(const RegulatoryElementConstPtrs& regelems) { From bcba2aae7774a5e6fec1428dde12fd67e3f2b6c3 Mon Sep 17 00:00:00 2001 From: Johannes Fischer Date: Fri, 22 Nov 2024 18:25:56 +0100 Subject: [PATCH 4/4] Add python tests for right of way and printing --- lanelet2_python/test/test_lanelet2.py | 96 ++++++++++++++++++++++++++- 1 file changed, 95 insertions(+), 1 deletion(-) diff --git a/lanelet2_python/test/test_lanelet2.py b/lanelet2_python/test/test_lanelet2.py index df13c236..4e3f65e2 100644 --- a/lanelet2_python/test/test_lanelet2.py +++ b/lanelet2_python/test/test_lanelet2.py @@ -1,6 +1,6 @@ import unittest import lanelet2 # if we fail here, there is something wrong with registration -from lanelet2.core import AttributeMap, getId, BasicPoint2d, Point3d, LineString3d, Lanelet, RegulatoryElement, TrafficLight, LaneletMap, createMapFromLanelets +from lanelet2.core import AttributeMap, getId, BasicPoint2d, Point3d, LineString3d, Lanelet, RegulatoryElement, TrafficLight, LaneletMap, createMapFromLanelets, RightOfWay from lanelet2.geometry import distance, intersects2d, boundingBox2d, to2D, intersection @@ -24,12 +24,53 @@ def getRegelem(): return TrafficLight(getId(), AttributeMap(), [getLineString()], getLineString()) +def getStopLine(): + stopLine = getLineString() + stopLine.attributes["type"] = "stop_line" + return stopLine + + +# need to pass lanelets as arguments, otherwise they are lost, resulting in +# RuntimeError: Nullptr passed to constructor! +def getRightOfWay(row_lanelets, yield_lanelets): + return RightOfWay(getId(), getAttributes(), row_lanelets, yield_lanelets, getStopLine()) + + def getLaneletMap(): lanelet = getLanelet() lanelet.addRegulatoryElement(getRegelem()) return createMapFromLanelets([lanelet]) +def getLaneletMapWithRightOfWay(): + yieldLanelet = getLanelet() + rightOfWayLanelet = getLanelet() + rightOfWay = getRightOfWay([rightOfWayLanelet], [yieldLanelet]) + rightOfWayLanelet.id = 2000 + rightOfWayLanelet.leftBound.id = 2001 + rightOfWayLanelet.leftBound[0].id = 3000 + rightOfWayLanelet.leftBound[1].id = 3001 + rightOfWayLanelet.rightBound.id = 2002 + rightOfWayLanelet.rightBound[0].id = 3003 + rightOfWayLanelet.rightBound[1].id = 3004 + yieldLanelet.id = 2003 + yieldLanelet.leftBound.id = 2004 + yieldLanelet.leftBound[0].id = 3010 + yieldLanelet.leftBound[1].id = 3011 + yieldLanelet.rightBound.id = 2005 + yieldLanelet.rightBound[0].id = 3013 + yieldLanelet.rightBound[1].id = 3014 + rightOfWay.id = 2006 + rightOfWay.stopLine.id = 2007 + rightOfWay.stopLine[0].id = 3020 + rightOfWay.stopLine[1].id = 3021 + rightOfWayLanelet.addRegulatoryElement(rightOfWay) + yieldLanelet.addRegulatoryElement(rightOfWay) + llmap = createMapFromLanelets([rightOfWayLanelet, yieldLanelet]) + llmap.add(rightOfWay) + return llmap + + def checkPrimitiveId(testClass, primitive): primitive.id = 30 testClass.assertEqual(primitive.id, 30) @@ -45,6 +86,46 @@ def checkPrimitiveAttributes(testClass, primitive): testClass.assertFalse("newkey" in primitive.attributes) +class LaneletReprTestCase(unittest.TestCase): + def test_lanelet_row_repr(self): + map = getLaneletMapWithRightOfWay() + self.assertEqual(len(map.regulatoryElementLayer), 1) + rightOfWay = map.regulatoryElementLayer[2006] + rightOfWayLanelet = map.laneletLayer[2000] + + rightOfWayParamsRepr = \ + "RuleParameterMap({" + \ + "'ref_line': [ConstLineString3d(2007, [" + \ + "ConstPoint3d(3020, 0, 0, 0, AttributeMap({'key': 'value'})), " + \ + "ConstPoint3d(3021, 0, 0, 0, AttributeMap({'key': 'value'}))" + \ + "], AttributeMap({'key': 'value', 'type': 'stop_line'}))], " + \ + "'right_of_way': [[id: 2000, left id: 2001, right id: 2002]], " + \ + "'yield': [[id: 2003, left id: 2004, right id: 2005]]" + \ + "})" + self.assertEqual(repr(rightOfWay.parameters), rightOfWayParamsRepr) + self.assertEqual(str(rightOfWay.parameters), "{ref_line: 2007 }{right_of_way: 2000 }{yield: 2003 }") + + rightOfWayRepr = "RightOfWay(2006, " + rightOfWayParamsRepr + \ + ", AttributeMap({'key': 'value', 'subtype': 'right_of_way', 'type': 'regulatory_element'}))" + self.assertEqual(repr(rightOfWay), rightOfWayRepr) + self.assertEqual(str(rightOfWay), "[id: 2006, parameters: {ref_line: 2007 }{right_of_way: 2000 }{yield: 2003 }]") + + rightOfWayLaneletRepr = \ + "Lanelet(2000, " + \ + "LineString3d(2001, [" + \ + "Point3d(3000, 0, 0, 0, AttributeMap({'key': 'value'})), " + \ + "Point3d(3001, 0, 0, 0, AttributeMap({'key': 'value'}))" + \ + "], AttributeMap({'key': 'value'})), " + \ + "LineString3d(2002, [" + \ + "Point3d(3003, 0, 0, 0, AttributeMap({'key': 'value'})), " + \ + "Point3d(3004, 0, 0, 0, AttributeMap({'key': 'value'}))" + \ + "], AttributeMap({'key': 'value'})), " + \ + "AttributeMap({'key': 'value'}), " + \ + "[" + rightOfWayRepr + "])" + self.assertEqual(repr(rightOfWayLanelet), rightOfWayLaneletRepr) + self.assertEqual(str(rightOfWayLanelet), "[id: 2000, left id: 2001, right id: 2002]") + + class LaneletApiTestCase(unittest.TestCase): def test_lanelet_id(self): checkPrimitiveId(self, getLanelet()) @@ -66,6 +147,19 @@ def test_lanelet_regelem(self): self.assertEqual(len(llt.trafficSigns()), 0) self.assertEqual(len(llt.regulatoryElements), 1) + def test_lanelet_row(self): + rightOfWayLanelet = getLanelet() + yieldLanelet = getLanelet() + rightOfWay = getRightOfWay([rightOfWayLanelet], [yieldLanelet]) + rightOfWayLanelet.addRegulatoryElement(rightOfWay) + yieldLanelet.addRegulatoryElement(rightOfWay) + self.assertEqual(len(rightOfWayLanelet.rightOfWay()), 1) + self.assertEqual(len(rightOfWayLanelet.regulatoryElements), 1) + self.assertEqual(len(yieldLanelet.rightOfWay()), 1) + self.assertEqual(len(yieldLanelet.regulatoryElements), 1) + self.assertEqual(len(rightOfWay.rightOfWayLanelets()), 1) + self.assertEqual(len(rightOfWay.yieldLanelets()), 1) + class LaneletMapApiTestCase(unittest.TestCase): def test_lanelet_map_basic(self):