From a796780c85e1ba31bb2efa48b612d8845cb38407 Mon Sep 17 00:00:00 2001 From: "Weiss, David" Date: Thu, 22 Oct 2020 13:33:44 +0200 Subject: [PATCH 1/3] fix(Importer): Make some attibutes of Object and Signal optional Signed-off-by: Weiss, David --- sim/include/roadInterface/roadElementTypes.h | 4 +- .../core/slave/importer/sceneryImporter.cpp | 55 +++++++------------ 2 files changed, 21 insertions(+), 38 deletions(-) diff --git a/sim/include/roadInterface/roadElementTypes.h b/sim/include/roadInterface/roadElementTypes.h index 7ad41a2c..0b37906e 100644 --- a/sim/include/roadInterface/roadElementTypes.h +++ b/sim/include/roadInterface/roadElementTypes.h @@ -1,5 +1,5 @@ /******************************************************************************* -* Copyright (c) 2017, 2018, 2019 in-tech GmbH +* Copyright (c) 2017, 2018, 2019, 2020 in-tech GmbH * 2016, 2017, 2018 ITK Engineering GmbH * * This program and the accompanying materials are made @@ -265,7 +265,7 @@ struct RoadObjectSpecification // http://www.opendrive.org/docs/OpenDRIVEFormatS double t {0}; double zOffset {0}; double validLength {0}; - RoadElementOrientation orientation; + RoadElementOrientation orientation{RoadElementOrientation::positive}; double width {0}; double length {0}; double radius {0}; diff --git a/sim/src/core/slave/importer/sceneryImporter.cpp b/sim/src/core/slave/importer/sceneryImporter.cpp index 92f5d3de..4fdc3fa4 100644 --- a/sim/src/core/slave/importer/sceneryImporter.cpp +++ b/sim/src/core/slave/importer/sceneryImporter.cpp @@ -1,5 +1,5 @@ /******************************************************************************* -* Copyright (c) 2017, 2018, 2019 in-tech GmbH +* Copyright (c) 2017, 2018, 2019, 2020 in-tech GmbH * 2016, 2017, 2018 ITK Engineering GmbH * Copyright (c) 2020 HLRS, University of Stuttgart. * @@ -693,7 +693,6 @@ void SceneryImporter::checkRoadSignalBoundaries(RoadSignalSpecification signal) ThrowIfFalse((signal.s >= 0 && (signal.dynamic == "yes" || signal.dynamic == "no") && (signal.orientation == "+" || signal.orientation == "-" || signal.orientation == "none") && - !signal.country.empty() && signal.height >= 0 && signal.width >= 0), "Invalid road signal boundaries."); @@ -720,20 +719,12 @@ void SceneryImporter::ParseSignals(QDomElement& roadElement, signalElement, "Attribute " + std::string(ATTRIBUTE::t) + " is missing."); ThrowIfFalse(ParseAttributeDouble(signalElement, ATTRIBUTE::zOffset, signal.zOffset), signalElement, "Attribute " + std::string(ATTRIBUTE::zOffset) + " is missing."); - ThrowIfFalse(ParseAttributeDouble(signalElement, ATTRIBUTE::hOffset, signal.hOffset), - signalElement, "Attribute " + std::string(ATTRIBUTE::hOffset) + " is missing."); - ThrowIfFalse(ParseAttributeDouble(signalElement, ATTRIBUTE::pitch, signal.pitch), - signalElement, "Attribute " + std::string(ATTRIBUTE::pitch) + " is missing."); ThrowIfFalse(ParseAttributeString(signalElement, ATTRIBUTE::id, signal.id), signalElement, "Attribute " + std::string(ATTRIBUTE::id) + " is missing."); - ThrowIfFalse(ParseAttributeString(signalElement, ATTRIBUTE::name, signal.name), - signalElement, "Attribute " + std::string(ATTRIBUTE::name) + " is missing."); ThrowIfFalse(ParseAttributeString(signalElement, ATTRIBUTE::dynamic, signal.dynamic, "no"), signalElement, "Attribute " + std::string(ATTRIBUTE::dynamic) + " is missing."); ThrowIfFalse(ParseAttributeString(signalElement, ATTRIBUTE::orientation, signal.orientation), signalElement, "Attribute " + std::string(ATTRIBUTE::orientation) + " is missing."); - ThrowIfFalse(ParseAttributeString(signalElement, ATTRIBUTE::country, signal.country), - signalElement, "Attribute " + std::string(ATTRIBUTE::country) + " is missing."); ThrowIfFalse(ParseAttributeString(signalElement, ATTRIBUTE::type, signal.type), signalElement, "Attribute " + std::string(ATTRIBUTE::type) + " is missing."); ThrowIfFalse(ParseAttributeString(signalElement, ATTRIBUTE::subtype, signal.subtype), @@ -741,14 +732,15 @@ void SceneryImporter::ParseSignals(QDomElement& roadElement, // optional std::string signalUnit; ParseAttributeDouble(signalElement, ATTRIBUTE::value, signal.value); - ParseAttributeString(signalElement, ATTRIBUTE::unit, signalUnit); + ParseAttributeString(signalElement, ATTRIBUTE::unit, signalUnit); ParseSignalUnit(signalUnit, signal.unit); - - // not generated by ODDlot - // potentially optional - no clue from the standard - ParseAttributeDouble(signalElement, ATTRIBUTE::height, signal.height); - ParseAttributeDouble(signalElement, ATTRIBUTE::width, signal.width); - ParseAttributeString(signalElement, ATTRIBUTE::text, signal.text); + ParseAttributeDouble(signalElement, ATTRIBUTE::hOffset, signal.hOffset); + ParseAttributeDouble(signalElement, ATTRIBUTE::pitch, signal.pitch); + ParseAttributeString(signalElement, ATTRIBUTE::name, signal.name); + ParseAttributeDouble(signalElement, ATTRIBUTE::height, signal.height); + ParseAttributeDouble(signalElement, ATTRIBUTE::width, signal.width); + ParseAttributeString(signalElement, ATTRIBUTE::text, signal.text); + ParseAttributeString(signalElement, ATTRIBUTE::country, signal.country); // check validity subtag ParseElementValidity(signalElement, signal.validity); @@ -807,9 +799,8 @@ void SceneryImporter::ParseObject(QDomElement& objectElement, RoadInterface* roa RoadObjectSpecification object; ThrowIfFalse(ParseAttributeType(objectElement, ATTRIBUTE::type, object.type), - objectElement, "Attribute " + std::string(ATTRIBUTE::type) + " is missing."); - ThrowIfFalse(ParseAttributeString(objectElement, ATTRIBUTE::name, object.name), - objectElement, "Attribute " + std::string(ATTRIBUTE::name) + " is missing."); + objectElement, "Attribute " + std::string(ATTRIBUTE::type) + " is missing or unknown."); + ParseAttributeString(objectElement, ATTRIBUTE::name, object.name); ThrowIfFalse(ParseAttributeString(objectElement, ATTRIBUTE::id, object.id), objectElement, "Attribute " + std::string(ATTRIBUTE::id) + " is missing."); ThrowIfFalse(ParseAttributeDouble(objectElement, ATTRIBUTE::s, object.s), @@ -818,22 +809,14 @@ void SceneryImporter::ParseObject(QDomElement& objectElement, RoadInterface* roa objectElement, "Attribute " + std::string(ATTRIBUTE::t) + " is missing."); ThrowIfFalse(ParseAttributeDouble(objectElement, ATTRIBUTE::zOffset, object.zOffset), objectElement, "Attribute " + std::string(ATTRIBUTE::zOffset) + " is missing."); - ThrowIfFalse(ParseAttributeDouble(objectElement, ATTRIBUTE::validLength, object.validLength), - objectElement, "Attribute " + std::string(ATTRIBUTE::validLength) + " is missing."); - ThrowIfFalse(ParseAttributeType(objectElement, ATTRIBUTE::orientation, object.orientation), - objectElement, "Attribute " + std::string(ATTRIBUTE::orientation) + " is missing."); - ThrowIfFalse(ParseAttributeDouble(objectElement, ATTRIBUTE::width, object.width), - objectElement, "Attribute " + std::string(ATTRIBUTE::width) + " is missing."); - ThrowIfFalse(ParseAttributeDouble(objectElement, ATTRIBUTE::length, object.length), - objectElement, "Attribute " + std::string(ATTRIBUTE::length) + " is missing."); - ThrowIfFalse(ParseAttributeDouble(objectElement, ATTRIBUTE::height, object.height), - objectElement, "Attribute " + std::string(ATTRIBUTE::height) + " is missing."); - ThrowIfFalse(ParseAttributeDouble(objectElement, ATTRIBUTE::hdg, object.hdg), - objectElement, "Attribute " + std::string(ATTRIBUTE::hdg) + " is missing."); - ThrowIfFalse(ParseAttributeDouble(objectElement, ATTRIBUTE::pitch, object.pitch), - objectElement, "Attribute " + std::string(ATTRIBUTE::pitch) + " is missing."); - ThrowIfFalse(ParseAttributeDouble(objectElement, ATTRIBUTE::roll, object.roll), - objectElement, "Attribute " + std::string(ATTRIBUTE::roll) + " is missing."); + ParseAttributeDouble(objectElement, ATTRIBUTE::validLength, object.validLength); + ParseAttributeType(objectElement, ATTRIBUTE::orientation, object.orientation); + ParseAttributeDouble(objectElement, ATTRIBUTE::width, object.width); + ParseAttributeDouble(objectElement, ATTRIBUTE::length, object.length); + ParseAttributeDouble(objectElement, ATTRIBUTE::height, object.height); + ParseAttributeDouble(objectElement, ATTRIBUTE::hdg, object.hdg); + ParseAttributeDouble(objectElement, ATTRIBUTE::pitch, object.pitch); + ParseAttributeDouble(objectElement, ATTRIBUTE::roll, object.roll); ParseAttributeDouble(objectElement, "radius", object.radius); ThrowIfFalse(object.checkStandardCompliance(), objectElement, "limits of object are not valid for openDrive standard"); -- GitLab From 90def43df0bd53df7b26a9948be7a37be8e8eceb Mon Sep 17 00:00:00 2001 From: "Weiss, David" Date: Thu, 22 Oct 2020 13:55:34 +0200 Subject: [PATCH 2/3] fix(World_OSI):Fix rounding error in GeometryConverter Signed-off-by: Weiss, David --- .../slave/modules/World_OSI/GeometryConverter.cpp | 13 ++++++++----- .../slave/modules/World_OSI/GeometryConverter.h | 12 +++++++----- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/sim/src/core/slave/modules/World_OSI/GeometryConverter.cpp b/sim/src/core/slave/modules/World_OSI/GeometryConverter.cpp index d5ac34fc..c1a0e85f 100644 --- a/sim/src/core/slave/modules/World_OSI/GeometryConverter.cpp +++ b/sim/src/core/slave/modules/World_OSI/GeometryConverter.cpp @@ -122,13 +122,17 @@ SampledGeometry GeometryConverter::CalculateSectionBetweenRoadMarkChanges(double std::list roadGeometries = road->GetGeometries(); bool firstGeometry{true}; - for(RoadGeometryInterface* roadGeometry: roadGeometries) + for(auto roadGeometry = roadGeometries.cbegin(); roadGeometry != roadGeometries.cend(); ++roadGeometry) { + auto next = std::next(roadGeometry); + double roadGeometryLength = (next == roadGeometries.end()) ? (*roadGeometry)->GetLength() + : (*next)->GetS() - (*roadGeometry)->GetS(); auto sampledGeometry = CalculateGeometry(roadSectionStart, roadSectionEnd, road, roadSection, - roadGeometry); + *roadGeometry, + roadGeometryLength); if (sampledGeometry.borderPoints.empty()) { continue; @@ -150,10 +154,9 @@ SampledGeometry GeometryConverter::CalculateGeometry(double roadSectionStart, double roadSectionEnd, const RoadInterface* road, const RoadLaneSectionInterface *roadSection, - const RoadGeometryInterface* roadGeometry) + const RoadGeometryInterface* roadGeometry, + double roadGeometryLength) { - double roadGeometryLength = roadGeometry->GetLength(); - auto roadGeometryStart = roadGeometry->GetS(); auto roadGeometryEnd = roadGeometryStart + roadGeometryLength; diff --git a/sim/src/core/slave/modules/World_OSI/GeometryConverter.h b/sim/src/core/slave/modules/World_OSI/GeometryConverter.h index 1f6180fe..4eb692c2 100644 --- a/sim/src/core/slave/modules/World_OSI/GeometryConverter.h +++ b/sim/src/core/slave/modules/World_OSI/GeometryConverter.h @@ -88,16 +88,18 @@ SampledGeometry CalculateSectionBetweenRoadMarkChanges(double roadSectionStart, //! Samples the lane boundary points of one geometry for one section with a defined maximum lateral error //! //! \param roadSectionStart start s coordinate of the section / first RoadMark change -//! \param roadSectionEnd end s coordinate of the section / second RoadMark change -//! \param road road the section is part of -//! \param roadSection section to convert -//! \param roadGeometry geometry to sample +//! \param roadSectionEnd end s coordinate of the section / second RoadMark change +//! \param road road the section is part of +//! \param roadSection section to convert +//! \param roadGeometry geometry to sample +//! \param roadGeometryLength length of the geometry //! \return sampled boundary points SampledGeometry CalculateGeometry(double roadSectionStart, double roadSectionEnd, const RoadInterface *road, const RoadLaneSectionInterface *roadSection, - const RoadGeometryInterface *roadGeometry); + const RoadGeometryInterface *roadGeometry, + double roadGeometryLength); //! Calculates the start s offset of a geometry inside a section //! -- GitLab From d8274567669dcf10d91ac4a733186c0cb1904d75 Mon Sep 17 00:00:00 2001 From: "Weiss, David" Date: Mon, 28 Sep 2020 12:44:54 +0200 Subject: [PATCH 3/3] feat(World_OSI): Add pedestrian crossing This commit also fixes some old enumeration values of OpenDRIVE 1.4 to 1.6 Signed-off-by: Weiss, David --- .../Development/FrameworkModules.md | 3 ++ sim/include/roadInterface/roadElementTypes.h | 39 +++++++------- sim/src/common/worldDefinitions.h | 1 + .../core/slave/importer/sceneryImporter.cpp | 26 ++++++---- .../modules/World_OSI/GeometryConverter.cpp | 1 + .../slave/modules/World_OSI/OWL/DataTypes.cpp | 32 ++++++++++++ .../slave/modules/World_OSI/OWL/DataTypes.h | 9 ++++ .../World_OSI/OWL/OpenDriveTypeMapper.h | 4 +- .../World_OSI/OWL/fakes/fakeRoadMarking.h | 2 + .../modules/World_OSI/SceneryConverter.cpp | 48 +++++++++++++---- .../modules/World_OSI/SceneryConverter.h | 7 +++ .../ImporterTest/IntegrationTestScenery.xodr | 51 ++++++++++--------- .../ImporterTest/SceneryLeftLaneEnds.xodr | 8 +-- .../SceneryImporter_IntegrationTests.cpp | 26 +++++++--- 14 files changed, 183 insertions(+), 74 deletions(-) diff --git a/sim/doc/DoxyGen/Function/Markdown/Simulation/Development/FrameworkModules.md b/sim/doc/DoxyGen/Function/Markdown/Simulation/Development/FrameworkModules.md index 7455fcb1..d3004cb3 100644 --- a/sim/doc/DoxyGen/Function/Markdown/Simulation/Development/FrameworkModules.md +++ b/sim/doc/DoxyGen/Function/Markdown/Simulation/Development/FrameworkModules.md @@ -353,8 +353,11 @@ The following road markings are supported: | RoadMarking | StVo Type | Subtype | Value and Units | |-----------------------------------------------|-----------|-------------|-------------------| +| PedestrianCrossing | 293 | - | - | | Stop line | 294 | - | - | +The pedestrian crossing can also be defined in OpenDRIVE as object with type "crosswalk". + \subsection dev_framework_modules_world_lanemarking Lane Markings The world also supports lane markings (i.e. printed lines between two lanes) according to the OpenDRIVE standard. diff --git a/sim/include/roadInterface/roadElementTypes.h b/sim/include/roadInterface/roadElementTypes.h index 0b37906e..c23872c9 100644 --- a/sim/include/roadInterface/roadElementTypes.h +++ b/sim/include/roadInterface/roadElementTypes.h @@ -75,7 +75,7 @@ enum class RoadLinkSideType //----------------------------------------------------------------------------- //! Type of lane //----------------------------------------------------------------------------- -enum class RoadLaneType // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 93 +enum class RoadLaneType // https://www.asam.net/standards/detail/opendrive/ { Undefined = 0, None, @@ -104,7 +104,7 @@ enum class RoadLaneType // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1 //----------------------------------------------------------------------------- //! Type of lane line //----------------------------------------------------------------------------- -enum class RoadLaneRoadMarkType // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 92 +enum class RoadLaneRoadMarkType // https://www.asam.net/standards/detail/opendrive/ { Undefined = 0, None, @@ -123,7 +123,7 @@ enum class RoadLaneRoadMarkType // http://www.opendrive.org/docs/OpenDRIVEFormat //----------------------------------------------------------------------------- //! Lane description: left, right or center //----------------------------------------------------------------------------- -enum class RoadLaneRoadDescriptionType // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 59 +enum class RoadLaneRoadDescriptionType // https://www.asam.net/standards/detail/opendrive/ { Left, Right, @@ -133,7 +133,7 @@ enum class RoadLaneRoadDescriptionType // http://www.opendrive.org/docs/OpenDRIV //----------------------------------------------------------------------------- //! LaneChange of the lane line //----------------------------------------------------------------------------- -enum class RoadLaneRoadMarkLaneChange // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 59 +enum class RoadLaneRoadMarkLaneChange // https://www.asam.net/standards/detail/opendrive/ { Undefined = 0, None, @@ -145,7 +145,7 @@ enum class RoadLaneRoadMarkLaneChange // http://www.opendrive.org/docs/OpenDRIVE //----------------------------------------------------------------------------- //! Color of the road mark //----------------------------------------------------------------------------- -enum class RoadLaneRoadMarkColor // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 92 +enum class RoadLaneRoadMarkColor // https://www.asam.net/standards/detail/opendrive/ { Undefined = 0, Blue, @@ -157,7 +157,7 @@ enum class RoadLaneRoadMarkColor // http://www.opendrive.org/docs/OpenDRIVEForma }; //! Weight of the road mark -enum class RoadLaneRoadMarkWeight // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 92 +enum class RoadLaneRoadMarkWeight // https://www.asam.net/standards/detail/opendrive/ { Undefined = 0, Standard, @@ -174,7 +174,7 @@ enum class RoadElementOrientation //----------------------------------------------------------------------------- //! Units used by signals //----------------------------------------------------------------------------- -enum class RoadSignalUnit // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 12 +enum class RoadSignalUnit // https://www.asam.net/standards/detail/opendrive/ { Undefined = 0, Meter, @@ -235,25 +235,28 @@ enum class RoadObjectType none = -1, obstacle, car, - truck, + pole, + tree, + vegetation, + barrier, + building, + parkingSpace, + patch, + railing, + trafficIsland, + crosswalk, + streetlamp, + gantry, + soundBarrier, van, bus, trailer, bike, motorbike, - tram, train, pedestrian, - pole, - tree, - vegetation, - barrier, - building, - parkingSpace, wind, - patch, - guardRail, - roadSideMarkerPost + roadMark }; struct RoadObjectSpecification // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 65 diff --git a/sim/src/common/worldDefinitions.h b/sim/src/common/worldDefinitions.h index ac9fb7a3..69dc21db 100644 --- a/sim/src/common/worldDefinitions.h +++ b/sim/src/common/worldDefinitions.h @@ -318,6 +318,7 @@ enum Type OvertakingBanEnd = 280, OvertakingBanTrucksEnd = 281, EndOffAllSpeedLimitsAndOvertakingRestrictions = 282, + PedestrianCrossing = 293, RightOfWayNextIntersection = 301, RightOfWayBegin = 306, RightOfWayEnd = 307, diff --git a/sim/src/core/slave/importer/sceneryImporter.cpp b/sim/src/core/slave/importer/sceneryImporter.cpp index 4fdc3fa4..bfc20d95 100644 --- a/sim/src/core/slave/importer/sceneryImporter.cpp +++ b/sim/src/core/slave/importer/sceneryImporter.cpp @@ -69,27 +69,31 @@ bool ParseType(const std::string &element, RoadElementOrientation &orientation) bool ParseType(const std::string &element, RoadObjectType &objectType) { return - assignIfMatching(element, objectType, "barrier", RoadObjectType::barrier) || + assignIfMatching(element, objectType, "none", RoadObjectType::none) || assignIfMatching(element, objectType, "obstacle", RoadObjectType::obstacle) || assignIfMatching(element, objectType, "car", RoadObjectType::car) || - assignIfMatching(element, objectType, "truck", RoadObjectType::truck) || + assignIfMatching(element, objectType, "pole", RoadObjectType::pole) || + assignIfMatching(element, objectType, "tree", RoadObjectType::tree) || + assignIfMatching(element, objectType, "vegetation", RoadObjectType::vegetation) || + assignIfMatching(element, objectType, "barrier", RoadObjectType::barrier) || + assignIfMatching(element, objectType, "building", RoadObjectType::building) || + assignIfMatching(element, objectType, "parkingSpace", RoadObjectType::parkingSpace) || + assignIfMatching(element, objectType, "patch", RoadObjectType::patch) || + assignIfMatching(element, objectType, "railing", RoadObjectType::railing) || + assignIfMatching(element, objectType, "trafficIsland", RoadObjectType::trafficIsland) || + assignIfMatching(element, objectType, "crosswalk", RoadObjectType::crosswalk) || + assignIfMatching(element, objectType, "streetlamp", RoadObjectType::streetlamp) || + assignIfMatching(element, objectType, "gantry", RoadObjectType::gantry) || + assignIfMatching(element, objectType, "soundBarrier", RoadObjectType::soundBarrier) || assignIfMatching(element, objectType, "van", RoadObjectType::van) || assignIfMatching(element, objectType, "bus", RoadObjectType::bus) || assignIfMatching(element, objectType, "trailer", RoadObjectType::trailer) || assignIfMatching(element, objectType, "bike", RoadObjectType::bike) || assignIfMatching(element, objectType, "motorbike", RoadObjectType::motorbike) || - assignIfMatching(element, objectType, "tram", RoadObjectType::tram) || assignIfMatching(element, objectType, "train", RoadObjectType::train) || assignIfMatching(element, objectType, "pedestrian", RoadObjectType::pedestrian)|| - assignIfMatching(element, objectType, "pole", RoadObjectType::pole) || - assignIfMatching(element, objectType, "tree", RoadObjectType::tree) || - assignIfMatching(element, objectType, "vegetation", RoadObjectType::vegetation) || - assignIfMatching(element, objectType, "building", RoadObjectType::building) || - assignIfMatching(element, objectType, "parkingSpace", RoadObjectType::parkingSpace) || assignIfMatching(element, objectType, "wind", RoadObjectType::wind) || - assignIfMatching(element, objectType, "patch", RoadObjectType::patch) || - assignIfMatching(element, objectType, "GuardRail", RoadObjectType::guardRail) || - assignIfMatching(element, objectType, "RoadSideMarkerPost", RoadObjectType::roadSideMarkerPost); + assignIfMatching(element, objectType, "roadMark", RoadObjectType::roadMark); } template diff --git a/sim/src/core/slave/modules/World_OSI/GeometryConverter.cpp b/sim/src/core/slave/modules/World_OSI/GeometryConverter.cpp index c1a0e85f..f0c1b481 100644 --- a/sim/src/core/slave/modules/World_OSI/GeometryConverter.cpp +++ b/sim/src/core/slave/modules/World_OSI/GeometryConverter.cpp @@ -125,6 +125,7 @@ SampledGeometry GeometryConverter::CalculateSectionBetweenRoadMarkChanges(double for(auto roadGeometry = roadGeometries.cbegin(); roadGeometry != roadGeometries.cend(); ++roadGeometry) { auto next = std::next(roadGeometry); + //To prevent rounding issues, the length is calculated as difference between the start s of the geometries double roadGeometryLength = (next == roadGeometries.end()) ? (*roadGeometry)->GetLength() : (*next)->GetS() - (*roadGeometry)->GetS(); auto sampledGeometry = CalculateGeometry(roadSectionStart, diff --git a/sim/src/core/slave/modules/World_OSI/OWL/DataTypes.cpp b/sim/src/core/slave/modules/World_OSI/OWL/DataTypes.cpp index b83c327d..09df0284 100644 --- a/sim/src/core/slave/modules/World_OSI/OWL/DataTypes.cpp +++ b/sim/src/core/slave/modules/World_OSI/OWL/DataTypes.cpp @@ -1558,6 +1558,38 @@ bool RoadMarking::SetSpecification(RoadSignalInterface* signal, Position positio return mapping_succeeded; } +bool RoadMarking::SetSpecification(RoadObjectInterface* object, Position position) +{ + bool mapping_succeeded = true; + + const auto baseStationary = osiSign->mutable_base(); + + baseStationary->mutable_position()->set_x(position.xPos); + baseStationary->mutable_position()->set_y(position.yPos); + baseStationary->mutable_position()->set_z(0.0); + baseStationary->mutable_dimension()->set_width(object->GetWidth()); + baseStationary->mutable_dimension()->set_length(object->GetLength()); + double yaw = object->GetHdg(); + yaw = CommonHelper::SetAngleToValidRange(yaw); + baseStationary->mutable_orientation()->set_yaw(yaw); + + const auto mutableOsiClassification = osiSign->mutable_classification(); + + mutableOsiClassification->set_type(osi3::RoadMarking_Classification_Type::RoadMarking_Classification_Type_TYPE_SYMBOLIC_TRAFFIC_SIGN); + mutableOsiClassification->set_monochrome_color(osi3::RoadMarking_Classification_Color::RoadMarking_Classification_Color_COLOR_WHITE); + if(object->GetType() == RoadObjectType::crosswalk) + { + mutableOsiClassification->set_traffic_main_sign_type(osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_ZEBRA_CROSSING); + } + else + { + mutableOsiClassification->set_traffic_main_sign_type(osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_OTHER); + mapping_succeeded = false; + } + + return mapping_succeeded; +} + CommonTrafficSign::Entity RoadMarking::GetSpecification(const double relativeDistance) const { CommonTrafficSign::Entity specification; diff --git a/sim/src/core/slave/modules/World_OSI/OWL/DataTypes.h b/sim/src/core/slave/modules/World_OSI/OWL/DataTypes.h index 25e2662b..82996e4d 100644 --- a/sim/src/core/slave/modules/World_OSI/OWL/DataTypes.h +++ b/sim/src/core/slave/modules/World_OSI/OWL/DataTypes.h @@ -706,6 +706,13 @@ public: //! \return true if succesfull, false if the road marking can not be converted virtual bool SetSpecification(RoadSignalInterface* signal, Position position) = 0; + //! Converts the specification imported from OpenDrive to OSI. + //! + //! \param signal OpenDrive specification + //! \param position position in the world + //! \return true if succesfull, false if the road marking can not be converted + virtual bool SetSpecification(RoadObjectInterface* signal, Position position) = 0; + /*! * \brief Copies the underlying OSI object to the given GroundTruth * @@ -1232,6 +1239,8 @@ public: virtual bool SetSpecification(RoadSignalInterface* signal, Position position) override; + virtual bool SetSpecification(RoadObjectInterface* object, Position position) override; + virtual bool IsValidForLane(OWL::Id laneId) const override; virtual void CopyToGroundTruth(osi3::GroundTruth& target) const override; diff --git a/sim/src/core/slave/modules/World_OSI/OWL/OpenDriveTypeMapper.h b/sim/src/core/slave/modules/World_OSI/OWL/OpenDriveTypeMapper.h index 5d5db48c..fc91b37a 100644 --- a/sim/src/core/slave/modules/World_OSI/OWL/OpenDriveTypeMapper.h +++ b/sim/src/core/slave/modules/World_OSI/OWL/OpenDriveTypeMapper.h @@ -175,6 +175,7 @@ namespace OpenDriveTypeMapper //! Conversion map from OpenDrive traffic sign to OSI for road markings const std::map roadMarkings = { + {"293", osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_ZEBRA_CROSSING}, {"294", osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_STOP}, }; @@ -226,6 +227,7 @@ namespace OpenDriveTypeMapper {osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_HIGHWAY_EXIT, CommonTrafficSign::Type::HighWayExit}, {osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_POLE_EXIT, CommonTrafficSign::Type::HighwayExitPole}, {osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_ANNOUNCE_RIGHT_LANE_END, CommonTrafficSign::Type::AnnounceRightLaneEnd}, - {osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_ANNOUNCE_LEFT_LANE_END, CommonTrafficSign::Type::AnnounceLeftLaneEnd} + {osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_ANNOUNCE_LEFT_LANE_END, CommonTrafficSign::Type::AnnounceLeftLaneEnd}, + {osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_ZEBRA_CROSSING, CommonTrafficSign::Type::PedestrianCrossing} }; }; diff --git a/sim/src/core/slave/modules/World_OSI/OWL/fakes/fakeRoadMarking.h b/sim/src/core/slave/modules/World_OSI/OWL/fakes/fakeRoadMarking.h index 29355fac..2495b325 100644 --- a/sim/src/core/slave/modules/World_OSI/OWL/fakes/fakeRoadMarking.h +++ b/sim/src/core/slave/modules/World_OSI/OWL/fakes/fakeRoadMarking.h @@ -35,6 +35,8 @@ public: void(OWL::Id)); MOCK_METHOD2(SetSpecification, bool(RoadSignalInterface*, Position position)); + MOCK_METHOD2(SetSpecification, + bool(RoadObjectInterface*, Position position)); MOCK_CONST_METHOD1(CopyToGroundTruth, void(osi3::GroundTruth&)); }; diff --git a/sim/src/core/slave/modules/World_OSI/SceneryConverter.cpp b/sim/src/core/slave/modules/World_OSI/SceneryConverter.cpp index 13d18eb4..b7dba69b 100644 --- a/sim/src/core/slave/modules/World_OSI/SceneryConverter.cpp +++ b/sim/src/core/slave/modules/World_OSI/SceneryConverter.cpp @@ -732,15 +732,22 @@ void SceneryConverter::CreateObjects() if (isOnRoad) { - OWL::Primitive::AbsPosition pos{x, y, 0}; - OWL::Primitive::Dimension dim{object->GetLength(), object->GetWidth(), object->GetHeight()}; - OWL::Primitive::AbsOrientation orientation{object->GetHdg(), object->GetPitch(), object->GetRoll()}; - new TrafficObjectAdapter(worldData, - localizer, - pos, - dim, - orientation, - object->GetId()); + if (object->GetType() == RoadObjectType::crosswalk) + { + CreateRoadMarking(object ,Position{x,y,yaw,0}, section->GetLanes()); + } + else + { + OWL::Primitive::AbsPosition pos{x, y, 0}; + OWL::Primitive::Dimension dim{object->GetLength(), object->GetWidth(), object->GetHeight()}; + OWL::Primitive::AbsOrientation orientation{object->GetHdg(), object->GetPitch(), object->GetRoll()}; + new TrafficObjectAdapter(worldData, + localizer, + pos, + dim, + orientation, + object->GetId()); + } } } } @@ -886,6 +893,29 @@ void SceneryConverter::CreateRoadMarking(RoadSignalInterface* signal, Position p } } +void SceneryConverter::CreateRoadMarking(RoadObjectInterface* object, Position position, const OWL::Interfaces::Lanes& lanes) +{ + OWL::Interfaces::RoadMarking& roadMarking = worldData.AddRoadMarking(); + + roadMarking.SetS(object->GetS()); + + if (!roadMarking.SetSpecification(object, position)) + { + const std::string message = "Unsupported traffic sign type: (id: " + object->GetId() + ")"; + LOG(CbkLogLevel::Warning, message); + return; + } + + for (auto lane : lanes) + { + OWL::OdId odId = worldData.GetLaneIdMapping().at(lane->GetId()); + if (object->IsValidForLane(odId)) + { + worldData.AssignRoadMarkingToLane(lane->GetId(), roadMarking); + } + } +} + std::tuple SceneryConverter::CalculateAbsoluteCoordinates(RoadInterface* road, OWL::CSection* section, const RoadObjectInterface* object) const diff --git a/sim/src/core/slave/modules/World_OSI/SceneryConverter.h b/sim/src/core/slave/modules/World_OSI/SceneryConverter.h index 4cd45548..95cc8f19 100644 --- a/sim/src/core/slave/modules/World_OSI/SceneryConverter.h +++ b/sim/src/core/slave/modules/World_OSI/SceneryConverter.h @@ -296,6 +296,13 @@ private: //! \param lanes lanes for which this road marking is valid void CreateRoadMarking(RoadSignalInterface* signal, Position position, const OWL::Interfaces::Lanes& lanes); + //! Creates a road marking in OWL from an OpenDrive RoadSignal + //! + //! \param object OpenDrive specification of the road marking as object + //! \param position position of the road marking in the world + //! \param lanes lanes for which this road marking is valid + void CreateRoadMarking(RoadObjectInterface* object, Position position, const OWL::Interfaces::Lanes& lanes); + void CreateTrafficLight(RoadSignalInterface* signal, RoadInterface* road); std::list GetRoadLanesAtDistance(RoadInterface *road, double s); diff --git a/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/Resources/ImporterTest/IntegrationTestScenery.xodr b/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/Resources/ImporterTest/IntegrationTestScenery.xodr index ab51d8c7..56746f74 100644 --- a/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/Resources/ImporterTest/IntegrationTestScenery.xodr +++ b/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/Resources/ImporterTest/IntegrationTestScenery.xodr @@ -184,30 +184,33 @@ This scenery consists of linear sections only. - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/Resources/ImporterTest/SceneryLeftLaneEnds.xodr b/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/Resources/ImporterTest/SceneryLeftLaneEnds.xodr index 850e429b..8c9808e9 100644 --- a/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/Resources/ImporterTest/SceneryLeftLaneEnds.xodr +++ b/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/Resources/ImporterTest/SceneryLeftLaneEnds.xodr @@ -105,16 +105,16 @@ - + - + - + - + diff --git a/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/SceneryImporter_IntegrationTests.cpp b/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/SceneryImporter_IntegrationTests.cpp index b7fc73b2..d40aa8f5 100644 --- a/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/SceneryImporter_IntegrationTests.cpp +++ b/sim/tests/integrationTests/OpenPassSlave_IntegrationTests/SceneryImporter_IntegrationTests.cpp @@ -945,13 +945,25 @@ TEST(SceneryImporter_IntegrationTests, SingleRoad_ImportWithCorrectTrafficSignGe ASSERT_THAT(trafficSign4.supplementary_sign(0).base().dimension().height(), DoubleEq(0.2)); ASSERT_THAT(trafficSign4.supplementary_sign(0).base().orientation().yaw(), DoubleEq(0.0)); - auto& roadMarking = groundtruth.road_marking(0); - ASSERT_THAT(roadMarking.base().position().x(), DoubleEq(41)); - ASSERT_THAT(roadMarking.base().position().y(), DoubleEq(0.5)); - ASSERT_THAT(roadMarking.base().position().z(), DoubleEq(0.0)); - ASSERT_THAT(roadMarking.base().dimension().width(), DoubleEq(4.0)); - ASSERT_THAT(roadMarking.base().dimension().height(), DoubleEq(0.0)); - ASSERT_THAT(roadMarking.base().orientation().yaw(), DoubleEq(0.0)); + ASSERT_THAT(groundtruth.road_marking_size(), Eq(2)); + + auto& roadMarking1 = groundtruth.road_marking(0); + ASSERT_THAT(roadMarking1.base().position().x(), DoubleEq(10)); + ASSERT_THAT(roadMarking1.base().position().y(), DoubleEq(7.5)); + ASSERT_THAT(roadMarking1.base().position().z(), DoubleEq(0.0)); + ASSERT_THAT(roadMarking1.base().dimension().width(), DoubleEq(8.0)); + ASSERT_THAT(roadMarking1.base().dimension().length(), DoubleEq(4.0)); + ASSERT_THAT(roadMarking1.base().orientation().yaw(), DoubleEq(0.0)); + ASSERT_THAT(roadMarking1.classification().traffic_main_sign_type(), Eq(osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_ZEBRA_CROSSING)); + + auto& roadMarking2 = groundtruth.road_marking(1); + ASSERT_THAT(roadMarking2.base().position().x(), DoubleEq(41)); + ASSERT_THAT(roadMarking2.base().position().y(), DoubleEq(0.5)); + ASSERT_THAT(roadMarking2.base().position().z(), DoubleEq(0.0)); + ASSERT_THAT(roadMarking2.base().dimension().width(), DoubleEq(4.0)); + ASSERT_THAT(roadMarking2.base().dimension().height(), DoubleEq(0.0)); + ASSERT_THAT(roadMarking2.base().orientation().yaw(), DoubleEq(0.0)); + ASSERT_THAT(roadMarking2.classification().traffic_main_sign_type(), Eq(osi3::TrafficSign_MainSign_Classification_Type::TrafficSign_MainSign_Classification_Type_TYPE_STOP)); } TEST(SceneryImporter_IntegrationTests, TJunction_ImportWithCorrectConnectionsAndPriorities) -- GitLab