diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7bfcd7a --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +__pycache__/ +*.pyc \ No newline at end of file diff --git a/Docs/OSCGenerator_Elevation.png b/Docs/OSCGenerator_Elevation.png new file mode 100644 index 0000000..851a647 Binary files /dev/null and b/Docs/OSCGenerator_Elevation.png differ diff --git a/Docs/OSCGenerator_Toolbar.png b/Docs/OSCGenerator_Toolbar.png index 62e34ac..a018832 100644 Binary files a/Docs/OSCGenerator_Toolbar.png and b/Docs/OSCGenerator_Toolbar.png differ diff --git a/Docs/OpenSCENARIO_Support.md b/Docs/OpenSCENARIO_Support.md new file mode 100644 index 0000000..bdcc5ad --- /dev/null +++ b/Docs/OpenSCENARIO_Support.md @@ -0,0 +1,135 @@ +## OpenSCENARIO Tags Support + +Based on [OpenSCENARIO 1.0.0](https://releases.asam.net/OpenSCENARIO/1.0.0/Model-Documentation/RenderedXsdOutput.html) + +- [OpenSCENARIO Tags Support](#openscenario-tags-support) +- [File Header](#file-header) +- [Catalog](#catalog) +- [Parameter Declarations](#parameter-declarations) +- [Road Network](#road-network) +- [Entities](#entities) + - [Scenario Object - Vehicle](#scenario-object---vehicle) + - [Scenario Object - Pedestrian](#scenario-object---pedestrian) + - [Scenario Object - MiscObject](#scenario-object---miscobject) +- [Actions - Global Actions](#actions---global-actions) +- [Actions - User Defined Actions](#actions---user-defined-actions) +- [Actions - Private Actions](#actions---private-actions) +- [Start / Stop Conditions - By Entity](#start--stop-conditions---by-entity) +- [Start / Stop Conditions - By Value](#start--stop-conditions---by-value) + +## File Header +| Tag | Import | Export | Remarks | +| ----------- | :----: | :----: | -------------------------------------- | +| revMajor | ❌ | 🟡 | Exports constant value | +| revMinor | ❌ | 🟡 | Exports constant value | +| date | ❌ | 🟡 | Exports time and date of file creation | +| description | ❌ | 🟡 | Exports constant value | +| author | ❌ | 🟡 | Exports constant value | + +## Catalog +❌ Catalog references are currently not supported for import and export + +## Parameter Declarations +⚠️ _Note: Local parameters are currently not supported, only global parameters._ +| Tag | Import | Export | Remarks | +| -------------------- | :----: | :----: | -------------------------- | +| ParameterDeclaration | ✅ | ✅ | Only for global parameters | + +## Road Network +| Tag | Import | Export | Remarks | +| ----------------------- | :----: | :----: | ------------------------------------------------------ | +| LogicFile | ✅ | ✅ | On import, will save information into `Metadata` layer | +| SceneGraphFile | ❌ | ❌ | Exports blank filepath | +| TrafficSignals | ❌ | ❌ | +| TrafficSignalController | ❌ | ❌ | + +## Entities +| Tag | Import | Export | Remarks | +| ---------------- | :----: | :----: | ----------------------------------------- | +| ScenarioObject | ✅ | ✅ | Entity `name` is not preserved on import | +| ObjectController | ❌ | ❌ | +| CatalogReference | ❌ | ❌ | `CatalogReference` for `ObjectController` | +| Controller | ❌ | ❌ | +| CatalogReference | ❌ | ❌ | `CatalogReference` for `ScenarioObject` | + +### Scenario Object - Vehicle +| Tag | Import | Export | Remarks | +| --------------------- | :----: | :----: | ---------------------------------------------- | +| ParameterDeclarations | ❌ | ❌ | Only global parameters are supported | +| BoundingBox | 🟡 | 🟡 | Exports constant value | +| Performance | 🟡 | 🟡 | Exports constant value | +| Axles | 🟡 | 🟡 | Exports constant value | +| Properties | 🟡 | 🟡 | Exports `ego_vehicle` if it is ego, else empty | + +### Scenario Object - Pedestrian +| Tag | Import | Export | Remarks | +| --------------------- | :----: | :----: | ------------------------------------ | +| ParameterDeclarations | ❌ | ❌ | Only global parameters are supported | +| BoundingBox | 🟡 | 🟡 | Exports constant value | +| Performance | 🟡 | 🟡 | Exports constant value | + +### Scenario Object - MiscObject +| Tag | Import | Export | Remarks | +| --------------------- | :----: | :----: | ------------------------------------ | +| ParameterDeclarations | ❌ | ❌ | Only global parameters are supported | +| BoundingBox | 🟡 | 🟡 | Exports constant value | +| Performance | 🟡 | 🟡 | Exports constant value | + +## Actions - Global Actions +| Tag | Import | Export | Remarks | +| -------------------- | :----: | :----: | ----------------------- | +| EnvironmentActions | ✅ | ✅ | +| EntityAction | ❌ | ❌ | +| ParameterAction | ❌ | ❌ | +| InfrastructureAction | ✅ | ✅ | Controls traffic lights | +| TrafficAction | ❌ | ❌ | + +## Actions - User Defined Actions +| Tag | Import | Export | Remarks | +| ------------------- | :----: | :----: | ------- | +| CustomCommandAction | ❌ | ❌ | + +## Actions - Private Actions +| Tag | Import | Export | Remarks | +| ----------------------------------------------- | :----: | :----: | --------------------------------- | +| LongitudinalAction / SpeedAction | ✅ | ✅ | +| LongitudinalAction / LongitudinalDistanceAction | ✅ | ✅ | +| LateralAction / LaneChangeAction | ✅ | ✅ | +| LateralAction / LaneOffsetAction | ✅ | ✅ | +| LateralAction / LateralDistanceAction | ✅ | ✅ | +| VisibilityAction | ❌ | ❌ | +| SynchronizeAction | ❌ | ❌ | +| ActivateControllerAction | ❌ | ❌ | +| ControllerAction / AssignControllerAction | ✅ | ✅ | +| ControllerAction / OverrideControllerAction | 🟡 | 🟡 | Exports pre-determined values | +| TeleportAction | 🟡 | 🟡 | Only `WorldPosition` is supported | +| RoutingAction / AssignRouteAction | ✅ | ✅ | +| RoutingAction / FollowTrajectoryAction | ❌ | ❌ | +| RoutingAction / AcquitePositionAction | ❌ | ❌ | + +## Start / Stop Conditions - By Entity +| Tag | Import | Export | Remarks | +| ------------------------- | :----: | :----: | ------- | +| EndOfRoadCondition | ✅ | ✅ | +| CollisionCondition | ✅ | ✅ | +| OffroadCondition | ✅ | ✅ | +| TimeHeadwayCondition | ✅ | ✅ | +| AccelerationCondition | ✅ | ✅ | +| StandStillCondition | ✅ | ✅ | +| SpeedContidion | ✅ | ✅ | +| RelativeSpeedCondition | ✅ | ✅ | +| TraveledDistanceCondition | ✅ | ✅ | +| ReachPositionCondition | ✅ | ✅ | +| DistanceCondition | ✅ | ✅ | +| RelativeDistanceCondition | ✅ | ✅ | + +## Start / Stop Conditions - By Value +| Tag | Import | Export | Remarks | +| -------------------------------- | :----: | :----: | ------------------------------------------------- | +| ParameterCondition | ✅ | ✅ | +| TimeOfDayCondition | ✅ | ✅ | +| SimulationTimeCondition | ✅ | ✅ | +| StoryboardElementStateCondition | ✅ | ✅ | Needs to manually specity storyboard element name | +| UserDefinedValueCondition | ✅ | ✅ | +| TrafficSignalCondition | ✅ | ✅ | +| TrafficSignalControllerCondition | ✅ | ✅ | diff --git a/README.md b/README.md index 068a114..c03d3d5 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,7 @@ ### Prerequisites - Python 3.6 - [QGIS 3](https://www.qgis.org/) -- [CARLA AD Map plugin version 2.4.2 and above](https://github.com/carla-simulator/map/releases) +- [CARLA AD Map plugin version 2.4.5 and above](https://github.com/carla-simulator/map/releases) - [CARLA](https://github.com/carla-simulator/carla/releases/) (Version >= 0.9.10) - [CARLA Scenario Runner](https://github.com/carla-simulator/scenario_runner/releases) @@ -79,63 +79,65 @@ pip3 install --user -r /path/to/requirements.txt ## OpenSCENARIO Support List +_For the full support list, visit [OpenSCENARIO Tags Support](Docs/OpenSCENARIO_Support.md)_ + ✅ - Supported ❌ - Not supported ### Environment -Description | Support | Notes --- | -- | -- -Time of day | ✅ | Includes enabling animation of time -Cloud state | ✅ | -Fog visual range | ✅ | -Sun | ✅ | Includes intensity, azimuth, elevation -Precipitation | ✅ | Includes precipitation type and intensity +| Description | Support | Notes | +| ---------------- | ------- | ----------------------------------------- | +| Time of day | ✅ | Includes enabling animation of time | +| Cloud state | ✅ | +| Fog visual range | ✅ | +| Sun | ✅ | Includes intensity, azimuth, elevation | +| Precipitation | ✅ | Includes precipitation type and intensity | ### Actors -Description | Support | Notes --- | -- | -- -Spawn with World Position | ✅ | -Set initial speed | ✅ | -Set orientation | ✅ | Can either use lane heading, or manually specified -Set as ego vehicle | ✅ | Only for vehicles -Agent selection | ✅ | Only for ego vehicles (manual_control will require manually changing `Ego_1` tag to `hero`) -Agent parameter overrides | ❌ | Only for ego vehicles (overriding throttle, brake, clutch, parking brake, steering, gear) +| Description | Support | Notes | +| ------------------------- | ------- | ------------------------------------------------------------------------------------------- | +| Spawn with World Position | ✅ | +| Set initial speed | ✅ | +| Set orientation | ✅ | Can either use lane heading, or manually specified | +| Set as ego vehicle | ✅ | Only for vehicles | +| Agent selection | ✅ | Only for ego vehicles (manual_control will require manually changing `Ego_1` tag to `hero`) | +| Agent parameter overrides | ❌ | Only for ego vehicles (overriding throttle, brake, clutch, parking brake, steering, gear) | ### Static Objects (Props) -Description | Support | Notes --- | -- | -- -Spawn with World Position | ✅ | -Set object type | ✅ | -Set object mass | ✅ | -Set orientation | ✅ | Can either use lane heading, or manually specified -Enable/Disable physics | ✅ | +| Description | Support | Notes | +| ------------------------- | ------- | -------------------------------------------------- | +| Spawn with World Position | ✅ | +| Set object type | ✅ | +| Set object mass | ✅ | +| Set orientation | ✅ | Can either use lane heading, or manually specified | +| Enable/Disable physics | ✅ | ### End Evaluation KPIs -Description | Support | Notes --- | -- | -- -Running stop test | ✅ | -Running red light test | ✅ | -Wrong lane test | ✅ | -On sidewalk test | ✅ | -Keep lane test | ✅ | -Collision test | ✅ | -Driven distance test | ✅ | +| Description | Support | Notes | +| ---------------------- | ------- | ----- | +| Running stop test | ✅ | +| Running red light test | ✅ | +| Wrong lane test | ✅ | +| On sidewalk test | ✅ | +| Keep lane test | ✅ | +| Collision test | ✅ | +| Driven distance test | ✅ | ### Entity Maneuvers -Description | Support | Notes --- | -- | -- -Waypoints | ✅ | -Longitudinal | ✅ | -Lateral | ✅ | -Synchronize Action | ❌ | -Teleport Action | ❌ | +| Description | Support | Notes | +| ------------------ | ------- | ----- | +| Waypoints | ✅ | +| Longitudinal | ✅ | +| Lateral | ✅ | +| Synchronize Action | ❌ | +| Teleport Action | ❌ | ### Global Maneuvers -Description | Support | Notes --- | -- | -- -Environment | ❌ | -Infrastructure Action | ✅ | Traffic light signal control +| Description | Support | Notes | +| --------------------- | ------- | ---------------------------- | +| Environment | ❌ | +| Infrastructure Action | ✅ | Traffic light signal control | ## Guide @@ -146,18 +148,19 @@ Infrastructure Action | ✅ | Traffic light signal control ![OpenSCENARIO Generator Toolbar](Docs/OSCGenerator_Toolbar.png) -Icon | Description --- | -- -Edit environment | Edit environment -Add vehicles | Add vehicles -Add pedestrians | Add pedestrians -Add static objects | Add static objects -Add maneuvers | Add maneuvers -Add parmeters | Add parameters -Add end evaluation KPIs | Add end evaluation KPIs (Specific for Scenario Runner) -Export OpenSCENARIO | Export OpenSCENARIO file -Connect to CARLA instance | Connect to carla instance -Insert camera | Add bird eye view camera +| Icon | Description | +| ---------------------------------------------------------------------------- | ------------------------------------------------------ | +| Edit environment | Edit environment | +| Add vehicles | Add vehicles | +| Add pedestrians | Add pedestrians | +| Add static objects | Add static objects | +| Add maneuvers | Add maneuvers | +| Add parmeters | Add parameters | +| Add end evaluation KPIs | Add end evaluation KPIs (Specific for Scenario Runner) | +| Export OpenSCENARIO | Export OpenSCENARIO file | +| Import OpenSCENARIO | Import OpenSCENARIO file | +| Connect to CARLA instance | Connect to carla instance | +| Insert camera | Add bird eye view camera | ### Adding Environment Variables @@ -183,6 +186,12 @@ _Note: To change environment settings, simply adjust the parameters and press 'A ![Selecting Lane ID](Docs/OSCGenerator_Vehicles_LaneID.png) +7. If click point contains lanes with multiple elevations, a dialog box will appear and ask to select the desired elevation. + + ![Selection Elevation](Docs/OSCGenerator_Elevation.png) + +_Note: Sometimes there will be a "Click point too far from lane" error. Try choosing a different elevation._ + _Note: Currently you can insert vehicle one at a time._ _Note: You can toggle labels on and off by clicking on the 'Label' button_ @@ -199,6 +208,12 @@ _Note: You can toggle labels on and off by clicking on the 'Label' button_ 6. If click point contains multiple lanes and 'Use lane heading' is enabled, a dialog box will appear and ask to select lane ID to use for lane heading. ![Selecting Lane ID](Docs/OSCGenerator_Vehicles_LaneID.png) + +7. If click point contains lanes with multiple elevations, a dialog box will appear and ask to select the desired elevation. + + ![Selection Elevation](Docs/OSCGenerator_Elevation.png) + +_Note: Sometimes there will be a "Click point too far from lane" error. Try choosing a different elevation._ _Note: You can insert as many pedestrians as you desire after pressing on 'Insert'_ diff --git a/carla_scenario_editor.py b/carla_scenario_editor.py index 9419d75..2ed9dff 100644 --- a/carla_scenario_editor.py +++ b/carla_scenario_editor.py @@ -24,9 +24,11 @@ from .osc_generator.add_pedestrians import AddPedestriansDockWidget from .osc_generator.add_static_objects import AddPropsDockWidget from .osc_generator.export_xosc import ExportXOSCDialog +from .osc_generator.import_xosc import ImportXOSCDialog from .osc_generator.edit_environment import EditEnvironmentDockWidget from .osc_generator.end_eval_criteria import EndEvalCriteriaDialog from .osc_generator.add_maneuvers import AddManeuversDockWidget +from .osc_generator.parameter_declarations import ParameterDeclarationsDockWidget try: import carla @@ -179,42 +181,52 @@ def add_action( def initGui(self): """Create the menu entries and toolbar icons inside the QGIS GUI.""" - self.weather = "icon_weather" - self.weather_info = "Edit Environment" - self.vehicle = "icon_vehicle" - self.vehicle_info = "Add Vehicles" - self.pedestrians = "icon_pedestrian" - self.pedestrians_info = "Add Pedestrians" - self.static = "icon_static" - self.static_info = "Add static objects" - self.maneuver = "icon_maneuver" - self.maneuver_info = "Add maneuvers" - self.parameter = "icon_parameter" - self.parameter_info = "Add Parameters" - self.endeval = "icon_endEval" - self.endeval_info = "End evaluation KPI's" - self.code = "icon_code" - self.code_info = "Export to open scenario" - self.carla = "carla_logo" - self.carla_info = "Connect to carla" - self.cam = "video_cam" - self.cam_info = "Add bird eye camera" - - self.__add_action__(self.weather, self.weather_info, self.edit_environment) - self.__add_action__(self.vehicle, self.vehicle_info, self.add_vehicles) - self.__add_action__(self.pedestrians, self.pedestrians_info, self.add_pedestrians) - self.__add_action__(self.static, self.static_info, self.add_props) - self.__add_action__(self.maneuver, self.maneuver_info, self.add_maneuver) - self.__add_action__(self.parameter, self.parameter_info, self.add_parameters) - self.__add_action__(self.endeval, self.endeval_info, self.end_evaluation) - self.__add_action__(self.code, self.code_info, self.export_xosc) - self.__add_action__(self.carla, self.carla_info, self.run_scenario) - self.__add_action__(self.cam, self.cam_info, self.add_camera_position) - - def __add_action__(self, text, info, callback): - "..." - action = self.ui.add_action(text, info, callback) - self.action_tool[text] = (action, None) + weather = "icon_weather" + weather_info = "Edit Environment" + vehicle = "icon_vehicle" + vehicle_info = "Add Vehicles" + pedestrians = "icon_pedestrian" + pedestrians_info = "Add Pedestrians" + static = "icon_static" + static_info = "Add static objects" + maneuver = "icon_maneuver" + maneuver_info = "Add maneuvers" + parameter = "icon_parameter" + parameter_info = "Add Parameters" + endeval = "icon_endEval" + endeval_info = "End evaluation KPI's" + code_export = "icon_code" + code_export_info = "Export OpenSCENARIO file" + code_import = "icon_import" + code_import_info = "Import OpenSCENARIO file" + carla = "carla_logo" + carla_info = "Connect to CARLA" + cam = "video_cam" + cam_info = "Add bird eye camera" + + self.__add_action__(weather, weather_info, self.edit_environment) + self.__add_action__(vehicle, vehicle_info, self.add_vehicles) + self.__add_action__(pedestrians, pedestrians_info, self.add_pedestrians) + self.__add_action__(static, static_info, self.add_props) + self.__add_action__(maneuver, maneuver_info, self.add_maneuver) + self.__add_action__(parameter, parameter_info, self.add_parameters) + self.__add_action__(endeval, endeval_info, self.end_evaluation) + self.__add_action__(code_export, code_export_info, self.export_xosc) + self.__add_action__(code_import, code_import_info, self.import_xosc) + self.__add_action__(carla, carla_info, self.run_scenario) + self.__add_action__(cam, cam_info, self.add_camera_position) + + def __add_action__(self, icon_file, info, callback): + """ + Adds action to QGIS toolbar + + Args: + icon_file (str): Icon file name (without file extension) + info (str): Tooltip information + callback (function): Callback function for button + """ + action = self.ui.add_action(icon_file, info, callback) + self.action_tool[icon_file] = (action, None) #-------------------------------------------------------------------------- @@ -325,6 +337,16 @@ def export_xosc(self): return_value = dlg.exec_() if return_value: ExportXOSCDialog.save_file(dlg) + + def import_xosc(self): + """ + Opens "Import OpenSCENARIO" dialog. + """ + dlg = ImportXOSCDialog() + dlg.show() + return_value = dlg.exec_() + if return_value: + ImportXOSCDialog.open_file(dlg) def edit_environment(self): """ @@ -484,16 +506,16 @@ def __init__(self, iface, title, toolbar_too=False): self.toolbar = self.iface.addToolBar(title) self.toolbar.setObjectName(title) - def add_action(self, text, info, callback): + def add_action(self, icon_file, info, callback): "..." parent = self.iface.mainWindow() - icon = self.__icon__(text) + icon = self.__icon__(icon_file) action = QAction(icon, info, parent) action.triggered.connect(callback) self.iface.addPluginToDatabaseMenu(self.menu, action) if self.toolbar is not None: self.toolbar.addAction(action) - self.actions[text] = action + self.actions[icon_file] = action return action def __icon__(self, text): diff --git a/icons/icon_import.png b/icons/icon_import.png new file mode 100644 index 0000000..b8dd014 Binary files /dev/null and b/icons/icon_import.png differ diff --git a/metadata.txt b/metadata.txt index 4b78453..db23f6b 100644 --- a/metadata.txt +++ b/metadata.txt @@ -6,7 +6,7 @@ name=OpenSCENARIO Editor Toolkit qgisMinimumVersion=3.0 description=This plugin allows users to create OpenSCENARIO scenarios and ability to run scenarios in CARLA. -version=0.1 +version=1.1 author=Intel Corp email=intel.com diff --git a/osc_generator/add_maneuvers.py b/osc_generator/add_maneuvers.py index 29d930e..0abddc7 100644 --- a/osc_generator/add_maneuvers.py +++ b/osc_generator/add_maneuvers.py @@ -11,14 +11,17 @@ import math # pylint: disable=no-name-in-module, no-member from qgis.PyQt import QtWidgets, uic -from qgis.PyQt.QtCore import Qt, pyqtSignal, QVariant +from qgis.PyQt.QtCore import Qt, pyqtSignal from qgis.gui import QgsMapTool from qgis.utils import iface -from qgis.core import (QgsProject, QgsVectorLayer, QgsMessageLog, Qgis, QgsField, - QgsFeature, QgsGeometry, QgsPalLayerSettings, QgsVectorLayerSimpleLabeling, - QgsTextFormat, QgsTextBackgroundSettings) +from qgis.core import (QgsProject, Qgis, QgsFeature, QgsGeometry, + QgsPalLayerSettings, QgsVectorLayerSimpleLabeling, QgsTextFormat, + QgsTextBackgroundSettings, QgsSpatialIndex, QgsFeatureRequest) from PyQt5.QtGui import QColor from PyQt5.QtWidgets import QInputDialog +from .helper_functions import (layer_setup_maneuvers_waypoint, layer_setup_maneuvers_and_triggers, + layer_setup_maneuvers_longitudinal, layer_setup_maneuvers_lateral, verify_parameters, is_float, + display_message) # AD Map plugin import ad_map_access as ad @@ -60,205 +63,22 @@ def __init__(self, parent=None): self.start_entity_choose_position_button.pressed.connect(self.get_world_position) self.stop_entity_choose_position_button.pressed.connect(self.get_world_position) - self._waypoint_layer = None - self._maneuver_layer = None - self._long_man_layer = None - self._lat_man_layer = None + layer_setup_maneuvers_waypoint() + layer_setup_maneuvers_and_triggers() + layer_setup_maneuvers_longitudinal() + layer_setup_maneuvers_lateral() + self._waypoint_layer = QgsProject.instance().mapLayersByName("Waypoint Maneuvers")[0] + self._maneuver_layer = QgsProject.instance().mapLayersByName("Maneuvers")[0] + self._long_man_layer = QgsProject.instance().mapLayersByName("Longitudinal Maneuvers")[0] + self._lat_man_layer = QgsProject.instance().mapLayersByName("Lateral Maneuvers")[0] self._man_id = None self._traffic_labels_on = False self._traffic_labels_setup = False self._traffic_lights_layer = None - self.layer_setup() + self.refresh_entity() self.refresh_traffic_lights() - def layer_setup(self): - """ - Sets up layers for maneuvers - """ - root_layer = QgsProject.instance().layerTreeRoot() - osc_layer = root_layer.findGroup("OpenSCENARIO") - - # Waypoint maneuvers - if not QgsProject.instance().mapLayersByName("Waypoint Maneuvers"): - waypoint_layer = QgsVectorLayer("Point", "Waypoint Maneuvers", "memory") - QgsProject.instance().addMapLayer(waypoint_layer, False) - osc_layer.addLayer(waypoint_layer) - # Setup layer attributes - data_attributes = [QgsField("Maneuver ID", QVariant.Int), - QgsField("Entity", QVariant.String), - QgsField("Waypoint No", QVariant.Int), - QgsField("Orientation", QVariant.Double), - QgsField("Pos X", QVariant.Double), - QgsField("Pos Y", QVariant.Double), - QgsField("Route Strategy", QVariant.String)] - data_input = waypoint_layer.dataProvider() - data_input.addAttributes(data_attributes) - waypoint_layer.updateFields() - - message = "Waypoint maneuvers layer added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - else: - message = "Using existing waypoint maneuver layer" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - - self._waypoint_layer = QgsProject.instance().mapLayersByName("Waypoint Maneuvers")[0] - label_settings = QgsPalLayerSettings() - label_settings.isExpression = True - label_name = "concat('ManID: ', \"Maneuver ID\", ' ', \"Entity\", ' - ', \"Waypoint No\")" - label_settings.fieldName = label_name - self._waypoint_layer.setLabeling(QgsVectorLayerSimpleLabeling(label_settings)) - self._waypoint_layer.setLabelsEnabled(True) - - # Maneuvers + Start Triggers + Stop Triggers - if not QgsProject.instance().mapLayersByName("Maneuvers"): - maneuver_layer = QgsVectorLayer("None", "Maneuvers", "memory") - QgsProject.instance().addMapLayer(maneuver_layer, False) - osc_layer.addLayer(maneuver_layer) - # Setup layer attributes - data_attributes = [QgsField("id", QVariant.Int), - QgsField("Maneuver Type", QVariant.String), - QgsField("Entity", QVariant.String), - QgsField("Entity: Maneuver Type", QVariant.String), - # Global Actions - QgsField("Global: Act Type", QVariant.String), - QgsField("Infra: Traffic Light ID", QVariant.Int), - QgsField("Infra: Traffic Light State", QVariant.String), - # Start Triggers - QgsField("Start Trigger", QVariant.String), - QgsField("Start - Entity: Condition", QVariant.String), - QgsField("Start - Entity: Ref Entity", QVariant.String), - QgsField("Start - Entity: Duration", QVariant.Double), - QgsField("Start - Entity: Value", QVariant.Double), - QgsField("Start - Entity: Rule", QVariant.String), - QgsField("Start - Entity: RelDistType", QVariant.String), - QgsField("Start - Entity: Freespace", QVariant.Bool), - QgsField("Start - Entity: Along Route", QVariant.Bool), - QgsField("Start - Value: Condition", QVariant.String), - QgsField("Start - Value: Param Ref", QVariant.String), - QgsField("Start - Value: Name", QVariant.String), - QgsField("Start - Value: DateTime", QVariant.String), - QgsField("Start - Value: Value", QVariant.Double), - QgsField("Start - Value: Rule", QVariant.String), - QgsField("Start - Value: State", QVariant.String), - QgsField("Start - Value: Sboard Type", QVariant.String), - QgsField("Start - Value: Sboard Element", QVariant.String), - QgsField("Start - Value: Sboard State", QVariant.String), - QgsField("Start - Value: TController Ref", QVariant.String), - QgsField("Start - Value: TController Phase", QVariant.String), - QgsField("Start - WorldPos: Tolerance", QVariant.Double), - QgsField("Start - WorldPos: X", QVariant.Double), - QgsField("Start - WorldPos: Y", QVariant.Double), - QgsField("Start - WorldPos: Heading", QVariant.Double), - # Stop Triggers - QgsField("Stop Trigger Enabled", QVariant.Bool), - QgsField("Stop Trigger", QVariant.String), - QgsField("Stop - Entity: Condition", QVariant.String), - QgsField("Stop - Entity: Ref Entity", QVariant.String), - QgsField("Stop - Entity: Duration", QVariant.Double), - QgsField("Stop - Entity: Value", QVariant.Double), - QgsField("Stop - Entity: Rule", QVariant.String), - QgsField("Stop - Entity: RelDistType", QVariant.String), - QgsField("Stop - Entity: Freespace", QVariant.Bool), - QgsField("Stop - Entity: Along Route", QVariant.Bool), - QgsField("Stop - Value: Condition", QVariant.String), - QgsField("Stop - Value: Param Ref", QVariant.String), - QgsField("Stop - Value: Name", QVariant.String), - QgsField("Stop - Value: DateTime", QVariant.String), - QgsField("Stop - Value: Value", QVariant.Double), - QgsField("Stop - Value: Rule", QVariant.String), - QgsField("Stop - Value: State", QVariant.String), - QgsField("Stop - Value: Sboard Type", QVariant.String), - QgsField("Stop - Value: Sboard Element", QVariant.String), - QgsField("Stop - Value: Sboard State", QVariant.String), - QgsField("Stop - Value: TController Ref", QVariant.String), - QgsField("Stop - Value: TController Phase", QVariant.String), - QgsField("Stop - WorldPos: Tolerance", QVariant.Double), - QgsField("Stop - WorldPos: X", QVariant.Double), - QgsField("Stop - WorldPos: Y", QVariant.Double), - QgsField("Stop - WorldPos: Heading", QVariant.Double)] - data_input = maneuver_layer.dataProvider() - data_input.addAttributes(data_attributes) - maneuver_layer.updateFields() - - message = "Maneuvers layer added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - else: - message = "Using existing maneuvers layer" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - - self._maneuver_layer = QgsProject.instance().mapLayersByName("Maneuvers")[0] - - # Longitudinal Maneuvers - if not QgsProject.instance().mapLayersByName("Longitudinal Maneuvers"): - long_man_layer = QgsVectorLayer("None", "Longitudinal Maneuvers", "memory") - QgsProject.instance().addMapLayer(long_man_layer, False) - osc_layer.addLayer(long_man_layer) - # Setup layer attributes - data_attributes = [QgsField("Maneuver ID", QVariant.Int), - QgsField("Type", QVariant.String), - QgsField("Speed Target", QVariant.String), - QgsField("Ref Entity", QVariant.String), - QgsField("Dynamics Shape", QVariant.String), - QgsField("Dynamics Dimension", QVariant.String), - QgsField("Dynamics Value", QVariant.String), - QgsField("Target Type", QVariant.String), - QgsField("Target Speed", QVariant.String), - QgsField("Continuous", QVariant.Bool), - QgsField("Freespace", QVariant.Bool), - QgsField("Max Acceleration", QVariant.String), - QgsField("Max Deceleration", QVariant.String), - QgsField("Max Speed", QVariant.String)] - data_input = long_man_layer.dataProvider() - data_input.addAttributes(data_attributes) - long_man_layer.updateFields() - - message = "Longitudinal maneuvers layer added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - else: - message = "Using existing longitudinal maneuvers layer" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - - self._long_man_layer = QgsProject.instance().mapLayersByName("Longitudinal Maneuvers")[0] - - # Lateral Maneuvers - if not QgsProject.instance().mapLayersByName("Lateral Maneuvers"): - lat_man_layer = QgsVectorLayer("None", "Lateral Maneuvers", "memory") - QgsProject.instance().addMapLayer(lat_man_layer, False) - osc_layer.addLayer(lat_man_layer) - # Setup layer attributes - data_attributes = [QgsField("Maneuver ID", QVariant.Int), - QgsField("Type", QVariant.String), - QgsField("Lane Target", QVariant.String), - QgsField("Ref Entity", QVariant.String), - QgsField("Dynamics Shape", QVariant.String), - QgsField("Dynamics Dimension", QVariant.String), - QgsField("Dynamics Value", QVariant.String), - QgsField("Lane Target Value", QVariant.String), - QgsField("Max Lateral Acceleration", QVariant.String), - QgsField("Max Acceleration", QVariant.String), - QgsField("Max Deceleration", QVariant.String), - QgsField("Max Speed", QVariant.String)] - data_input = lat_man_layer.dataProvider() - data_input.addAttributes(data_attributes) - lat_man_layer.updateFields() - - message = "Lateral maneuvers layer added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - else: - message = "Using existing lateral maneuvers layer" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - - self._lat_man_layer = QgsProject.instance().mapLayersByName("Lateral Maneuvers")[0] - def refresh_entity(self): """ Gets list of entities spawned on map and populates drop down @@ -363,6 +183,9 @@ def refresh_traffic_lights(self): for feature in layer.getFeatures(): loaded_traffic_light_ids = str(feature["Id"]) traffic_light_ids.append(loaded_traffic_light_ids) + + if len(traffic_light_ids) == 0: + traffic_light_ids.append("0") self.traffic_light_id.addItems(traffic_light_ids) @@ -850,6 +673,7 @@ def save_maneuver_attributes(self): float(self.start_entity_tolerance.text()), float(self.start_entity_position_x.text()), float(self.start_entity_position_y.text()), + float(self.start_entity_position_z.text()), float(self.start_entity_heading.text()), # Stop Triggers self.stop_triggers_group.isChecked(), @@ -877,69 +701,64 @@ def save_maneuver_attributes(self): float(self.stop_entity_tolerance.text()), float(self.stop_entity_position_x.text()), float(self.stop_entity_position_y.text()), + float(self.stop_entity_position_z.text()), float(self.stop_entity_heading.text())]) self._maneuver_layer.dataProvider().addFeature(feature) message = "Maneuver added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) + display_message(message, level="Info") def save_longitudinal_attributes(self): """ Gets longudinal maneuver attributes and saves into QGIS attributes table. """ - if self.is_float(self.long_dynamics_value.text()): + if is_float(self.long_dynamics_value.text()): long_dynamics_value = float(self.long_dynamics_value.text()) else: - verification = self.verify_parameters(self.long_dynamics_value.text()) + verification = verify_parameters(self.long_dynamics_value.text()) if len(verification) == 0: message = f"Parameter {self.long_dynamics_value.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: long_dynamics_value = self.long_dynamics_value.text() - if self.is_float(self.long_target_speed_value.text()): + if is_float(self.long_target_speed_value.text()): long_target_speed_value = float(self.long_target_speed_value.text()) else: - verification = self.verify_parameters(self.long_target_speed_value.text()) + verification = verify_parameters(self.long_target_speed_value.text()) if len(verification) == 0: message = f"Parameter {self.long_target_speed_value.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: long_target_speed_value = self.long_target_speed_value.text() - if self.is_float(self.long_max_accel.text()): + if is_float(self.long_max_accel.text()): long_max_accel = float(self.long_max_accel.text()) else: - verification = self.verify_parameters(self.long_max_accel.text()) + verification = verify_parameters(self.long_max_accel.text()) if len(verification) == 0: message = f"Parameter {self.long_max_accel.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: long_max_accel = self.long_max_accel.text() - if self.is_float(self.long_max_decel.text()): + if is_float(self.long_max_decel.text()): long_max_decel = float(self.long_max_decel.text()) else: - verification = self.verify_parameters(self.long_max_decel.text()) + verification = verify_parameters(self.long_max_decel.text()) if len(verification) == 0: message = f"Parameter {self.long_max_decel.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: long_max_decel = self.long_max_decel.text() - if self.is_float(self.long_max_speed.text()): + if is_float(self.long_max_speed.text()): long_max_speed = float(self.long_max_speed.text()) else: - verification = self.verify_parameters(self.long_max_speed.text()) + verification = verify_parameters(self.long_max_speed.text()) if len(verification) == 0: message = f"Parameter {self.long_max_speed.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: long_max_speed = self.long_max_speed.text() @@ -961,65 +780,59 @@ def save_longitudinal_attributes(self): self._long_man_layer.dataProvider().addFeature(feature) message = "Maneuver added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) + display_message(message, level="Info") def save_lateral_attributes(self): """ Gets lateral maneuver attributes and saves into QGIS attributes table. """ - if self.is_float(self.lateral_dynamics_value.text()): + if is_float(self.lateral_dynamics_value.text()): lateral_dynamics_value = float(self.lateral_dynamics_value.text()) else: - verification = self.verify_parameters(self.lateral_dynamics_value.text()) + verification = verify_parameters(self.lateral_dynamics_value.text()) if len(verification) == 0: message = f"Parameter {self.lateral_dynamics_value.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: lateral_dynamics_value = self.lateral_dynamics_value.text() - if self.is_float(self.lateral_max_lat_accel.text()): + if is_float(self.lateral_max_lat_accel.text()): lateral_max_lat_accel = float(self.lateral_max_lat_accel.text()) else: - verification = self.verify_parameters(self.lateral_max_lat_accel.text()) + verification = verify_parameters(self.lateral_max_lat_accel.text()) if len(verification) == 0: message = f"Parameter {self.lateral_max_lat_accel.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: lateral_max_lat_accel = self.lateral_max_lat_accel.text() - if self.is_float(self.lateral_max_accel.text()): + if is_float(self.lateral_max_accel.text()): lateral_max_accel = float(self.lateral_max_accel.text()) else: - verification = self.verify_parameters(self.lateral_max_accel.text()) + verification = verify_parameters(self.lateral_max_accel.text()) if len(verification) == 0: message = f"Parameter {self.lateral_max_accel.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: lateral_max_accel = self.lateral_max_accel.text() - if self.is_float(self.lateral_max_decel.text()): + if is_float(self.lateral_max_decel.text()): lateral_max_decel = float(self.lateral_max_decel.text()) else: - verification = self.verify_parameters(self.lateral_max_decel.text()) + verification = verify_parameters(self.lateral_max_decel.text()) if len(verification) == 0: message = f"Parameter {self.lateral_max_decel.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: lateral_max_decel = self.lateral_max_decel.text() - if self.is_float(self.lateral_max_speed.text()): + if is_float(self.lateral_max_speed.text()): lateral_max_speed = float(self.lateral_max_speed.text()) else: - verification = self.verify_parameters(self.lateral_max_speed.text()) + verification = verify_parameters(self.lateral_max_speed.text()) if len(verification) == 0: message = f"Parameter {self.lateral_max_speed.text()} does not exist!" - iface.messageBar().pushMessage("Critical", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: lateral_max_speed = self.lateral_max_speed.text() @@ -1039,46 +852,8 @@ def save_lateral_attributes(self): self._lat_man_layer.dataProvider().addFeature(feature) message = "Maneuver added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) + display_message(message, level="Info") - def verify_parameters(self, param): - """ - Checks Parameter Declarations attribute table to verify parameter exists - - Args: - param (string): name of parameter to check against - - Returns: - feature (dict): parameter definitions - """ - param_layer = QgsProject.instance().mapLayersByName("Parameter Declarations")[0] - query = f'"Parameter Name" = \'{param}\'' - feature_request = QgsFeatureRequest().setFilterExpression(query) - features = param_layer.getFeatures(feature_request) - feature = {} - - for feat in features: - feature["Type"] = feat["Type"] - feature["Value"] = feat["Value"] - - return feature - - def is_float(self, value): - """ - Checks value if it can be converted to float. - - Args: - value (string): value to check if can be converted to float - - Returns: - bool: True if float, False if not - """ - try: - float(value) - return True - except ValueError: - return False #pylint: disable=missing-function-docstring class PointTool(QgsMapTool): @@ -1109,10 +884,42 @@ def canvasReleaseEvent(self, event): # Get the click x = event.pos().x() y = event.pos().y() + point = self._canvas.getCoordinateTransform().toMapCoordinates(x, y) + lane_edge_layer = QgsProject.instance().mapLayersByName("Lane Edge")[0] + lane_edge_data_provider = lane_edge_layer.dataProvider() + spatial_index = QgsSpatialIndex() + spatial_feature = QgsFeature() + lane_edge_features = lane_edge_data_provider.getFeatures() + + while lane_edge_features.nextFeature(spatial_feature): + spatial_index.addFeature(spatial_feature) + + nearest_ids = spatial_index.nearestNeighbor(point, 5) + + z_values = set() + for feat in lane_edge_layer.getFeatures(QgsFeatureRequest().setFilterFids(nearest_ids)): + feature_coordinates = feat.geometry().vertexAt(1) + z_values.add(round(feature_coordinates.z(), ndigits=4)) + + if max(z_values) - min(z_values) < 0.1: + altitude = max(z_values) + else: + stringified_z_values = [str(z_value) for z_value in z_values] + z_value_selected, ok_pressed = QInputDialog.getItem( + QInputDialog(), + "Choose Elevation", + "Elevation (meters)", + tuple(stringified_z_values), + current=0, + editable=False) + + if ok_pressed: + altitude = float(z_value_selected) + # Converting to ENU points - geopoint = ad.map.point.createGeoPoint(longitude=point.x(), latitude=point.y(), altitude=0) + geopoint = ad.map.point.createGeoPoint(longitude=point.x(), latitude=point.y(), altitude=altitude) enupoint = ad.map.point.toENU(geopoint) add_entity_attr = AddManeuverAttributes() @@ -1134,6 +941,7 @@ def canvasReleaseEvent(self, event): float(processed_attrs["Orientation"]), float(enupoint.x), float(enupoint.y), + float(enupoint.z), processed_attrs["Route Strat"]]) feature.setGeometry(QgsGeometry.fromPointXY(point)) self._data_input.addFeature(feature) @@ -1141,9 +949,11 @@ def canvasReleaseEvent(self, event): heading = add_entity_attr.get_entity_heading(geopoint) self._parent.start_entity_position_x.setText(str(enupoint.x)) self._parent.start_entity_position_y.setText(str(enupoint.y)) + self._parent.start_entity_position_z.setText(str(enupoint.z)) self._parent.start_entity_heading.setText(str(heading)) self._parent.stop_entity_position_x.setText(str(enupoint.x)) self._parent.stop_entity_position_y.setText(str(enupoint.y)) + self._parent.stop_entity_position_z.setText(str(enupoint.z)) self._parent.stop_entity_heading.setText(str(heading)) self._canvas.unsetMapTool(self) @@ -1183,7 +993,7 @@ def get_entity_heading(self, geopoint): lane_heading: [float] heading of click point at selected lane ID lane_heading: [None] if click point is not valid """ - dist = ad.physics.Distance(0.025) + dist = ad.physics.Distance(1) admap_matched_points = ad.map.match.AdMapMatching.findLanes(geopoint, dist) lanes_detected = 0 @@ -1192,8 +1002,7 @@ def get_entity_heading(self, geopoint): if lanes_detected == 0: message = "Click point is too far from valid lane" - iface.messageBar().pushMessage("Error", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") return None elif lanes_detected == 1: for point in admap_matched_points: diff --git a/osc_generator/add_maneuvers_widget.ui b/osc_generator/add_maneuvers_widget.ui index 453320f..6ffbaa7 100644 --- a/osc_generator/add_maneuvers_widget.ui +++ b/osc_generator/add_maneuvers_widget.ui @@ -72,7 +72,7 @@ 0 - -356 + 0 410 992 @@ -843,7 +843,13 @@ - + + + + 0 + + + @@ -890,7 +896,7 @@ 0 0 363 - 1156 + 1179 @@ -1134,6 +1140,13 @@ Position Type + + + + Coordinate X + + + @@ -1169,14 +1182,14 @@ - + Heading - + Choose position @@ -1192,14 +1205,7 @@ - - - - Coordinate X - - - - + 0 @@ -1213,6 +1219,20 @@ + + + + Coordinate Z + + + + + + + 0 + + + @@ -1343,7 +1363,7 @@ - 20 + 18 0 0 2020 @@ -1360,7 +1380,7 @@ @@ -1578,7 +1598,7 @@ 0 0 363 - 1156 + 1179 @@ -1863,14 +1883,14 @@ - + Heading - + Choose position @@ -1893,7 +1913,7 @@ - + 0 @@ -1907,6 +1927,20 @@ + + + + Coordinate Z + + + + + + + 0 + + + @@ -2037,7 +2071,7 @@ - 18 + 16 0 0 2020 @@ -2054,7 +2088,7 @@ diff --git a/osc_generator/add_pedestrians.py b/osc_generator/add_pedestrians.py index 04e29e4..2919cab 100644 --- a/osc_generator/add_pedestrians.py +++ b/osc_generator/add_pedestrians.py @@ -12,13 +12,13 @@ import random # pylint: disable=no-name-in-module, no-member from PyQt5.QtWidgets import QInputDialog -from qgis.core import (Qgis, QgsFeature, QgsField, QgsGeometry, QgsMessageLog, - QgsPalLayerSettings, QgsPointXY, QgsProject, QgsVectorLayer, - QgsVectorLayerSimpleLabeling, QgsFeatureRequest) +from qgis.core import (Qgis, QgsFeature, QgsGeometry, QgsMessageLog, QgsPointXY, + QgsProject, QgsFeatureRequest, QgsSpatialIndex) from qgis.gui import QgsMapTool from qgis.PyQt import QtWidgets, uic -from qgis.PyQt.QtCore import Qt, QVariant, pyqtSignal +from qgis.PyQt.QtCore import Qt, pyqtSignal from qgis.utils import iface +from .helper_functions import layer_setup_walker, get_entity_heading, is_float, verify_parameters # AD Map plugin import ad_map_access as ad @@ -45,39 +45,7 @@ def __init__(self, parent=None): self.walker_labels_button.pressed.connect(self.toggle_labels) self._labels_on = True - self._walker_layer = None - self.layer_setup() - - def layer_setup(self): - """ - Sets up layer for pedestrians - """ - root_layer = QgsProject.instance().layerTreeRoot() - osc_layer = root_layer.findGroup("OpenSCENARIO") - if not QgsProject.instance().mapLayersByName("Pedestrians"): - walker_layer = QgsVectorLayer("Polygon", "Pedestrians", "memory") - QgsProject.instance().addMapLayer(walker_layer, False) - osc_layer.addLayer(walker_layer) - # Setup layer attributes - data_attributes = [QgsField("id", QVariant.Int), - QgsField("Walker", QVariant.String), - QgsField("Orientation", QVariant.Double), - QgsField("Pos X", QVariant.Double), - QgsField("Pos Y", QVariant.Double), - QgsField("Init Speed", QVariant.String)] - data_input = walker_layer.dataProvider() - data_input.addAttributes(data_attributes) - walker_layer.updateFields() - - label_settings = QgsPalLayerSettings() - label_settings.isExpression = True - label_settings.fieldName = "concat('Pedestrian_', \"id\")" - walker_layer.setLabeling(QgsVectorLayerSimpleLabeling(label_settings)) - walker_layer.setLabelsEnabled(True) - - iface.messageBar().pushMessage("Info", "Pedestrian layer added", level=Qgis.Info) - QgsMessageLog.logMessage("Pedestrian layer added", level=Qgis.Info) - + layer_setup_walker() self._walker_layer = QgsProject.instance().mapLayersByName("Pedestrians")[0] def toggle_labels(self): @@ -119,11 +87,11 @@ def insert_pedestrian(self): if self.walker_orientation_use_lane.isChecked(): orientation = None else: - if self.is_float(self.walker_orientation.text()): + if is_float(self.walker_orientation.text()): walker_orientation = float(self.walker_orientation.text()) walker_orientation = math.radians(walker_orientation) else: - verification = self.verify_parameters(param=self.walker_orientation.text()) + verification = verify_parameters(param=self.walker_orientation.text()) if len(verification) == 0: # UI Information message = f"Parameter {self.walker_orientation.text()} does not exist!" @@ -134,10 +102,10 @@ def insert_pedestrian(self): orientation = math.radians(orientation) init_speed = None - if self.is_float(self.walker_init_speed.text()): + if is_float(self.walker_init_speed.text()): init_speed = float(self.walker_init_speed.text()) else: - verification = self.verify_parameters(param=self.walker_init_speed.text()) + verification = verify_parameters(param=self.walker_init_speed.text()) if len(verification) == 0: # UI Information message = f"Parameter {self.walker_init_speed.text()} does not exist!" @@ -175,44 +143,6 @@ def override_orientation(self): else: self.walker_orientation.setEnabled(True) - def verify_parameters(self, param): - """ - Checks Parameter Declarations attribute table to verify parameter exists - - Args: - param (string): name of parameter to check against - - Returns: - feature (dict): parameter definitions - """ - param_layer = QgsProject.instance().mapLayersByName("Parameter Declarations")[0] - query = f'"Parameter Name" = \'{param}\'' - feature_request = QgsFeatureRequest().setFilterExpression(query) - features = param_layer.getFeatures(feature_request) - feature = {} - - for feat in features: - feature["Type"] = feat["Type"] - feature["Value"] = feat["Value"] - - return feature - - def is_float(self, value): - """ - Checks value if it can be converted to float. - - Args: - value (string): value to check if can be converted to float - - Returns: - bool: True if float, False if not - """ - try: - float(value) - return True - except ValueError: - return False - #pylint: disable=missing-function-docstring class PointTool(QgsMapTool): """Enables Point Addition""" @@ -242,14 +172,45 @@ def canvasReleaseEvent(self, event): point = self._canvas.getCoordinateTransform().toMapCoordinates(x, y) + lane_edge_layer = QgsProject.instance().mapLayersByName("Lane Edge")[0] + lane_edge_data_provider = lane_edge_layer.dataProvider() + spatial_index = QgsSpatialIndex() + spatial_feature = QgsFeature() + lane_edge_features = lane_edge_data_provider.getFeatures() + + while lane_edge_features.nextFeature(spatial_feature): + spatial_index.insertFeature(spatial_feature) + + nearest_ids = spatial_index.nearestNeighbor(point, 5) + + z_values = set() + for feat in lane_edge_layer.getFeatures(QgsFeatureRequest().setFilterFids(nearest_ids)): + feature_coordinates = feat.geometry().vertexAt(1) + z_values.add(round(feature_coordinates.z(), ndigits=4)) + + if max(z_values) - min(z_values) < 0.1: + altitude = max(z_values) + else: + stringified_z_values = [str(z_value) for z_value in z_values] + z_value_selected, ok_pressed = QInputDialog.getItem( + QInputDialog(), + "Choose Elevation", + "Elevation (meters)", + tuple(stringified_z_values), + current=0, + editable=False) + + if ok_pressed: + altitude = float(z_value_selected) + # Converting to ENU points - geopoint = ad.map.point.createGeoPoint(longitude=point.x(), latitude=point.y(), altitude=0) + geopoint = ad.map.point.createGeoPoint(longitude=point.x(), latitude=point.y(), altitude=altitude) enupoint = ad.map.point.toENU(geopoint) add_walker = AddPedestrianAttribute() # Get lane heading and save attribute (when not manually specified) if self._use_lane_heading is True: - self._pedestrian_attributes["Orientation"] = add_walker.get_pedestrian_heading(geopoint) + self._pedestrian_attributes["Orientation"] = get_entity_heading(geopoint) # Add points only if user clicks within lane boundaries (Orientation is not None) if self._pedestrian_attributes["Orientation"] is not None: @@ -266,6 +227,7 @@ def canvasReleaseEvent(self, event): pedestrian_attr["Orientation"], float(enupoint.x), float(enupoint.y), + float(enupoint.z) + 0.2, # Avoid ground collision pedestrian_attr["Init Speed"]]) feature.setGeometry(QgsGeometry.fromPolygonXY([polygon_points])) self._data_input.addFeature(feature) @@ -287,6 +249,7 @@ def isTransient(self): def isEditTool(self): return True + #pylint: enable=missing-function-docstring @@ -294,63 +257,6 @@ class AddPedestrianAttribute(): """ Class for processing / acquiring pedestrian attributes. """ - def get_pedestrian_heading(self, geopoint): - """ - Acquires heading based on spawn position in map. - Prompts user to select lane if multiple lanes exist at spawn position. - Throws error if spawn position is not on lane. - - Args: - geopoint: [AD Map GEOPoint] point of click event - - Returns: - lane_heading: [float] heading of click point at selected lane ID - lane_heading: [None] if click point is not valid - """ - dist = ad.physics.Distance(0.025) - admap_matched_points = ad.map.match.AdMapMatching.findLanes(geopoint, dist) - - lanes_detected = 0 - for point in admap_matched_points: - lanes_detected += 1 - - if lanes_detected == 0: - message = "Click point is too far from valid lane" - iface.messageBar().pushMessage("Error", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) - return None - elif lanes_detected == 1: - for point in admap_matched_points: - lane_id = point.lanePoint.paraPoint.laneId - para_offset = point.lanePoint.paraPoint.parametricOffset - parapoint = ad.map.point.createParaPoint(lane_id, para_offset) - lane_heading = ad.map.lane.getLaneENUHeading(parapoint) - return lane_heading - else: - lane_ids_to_match = [] - lane_id = [] - para_offsets = [] - for point in admap_matched_points: - lane_ids_to_match.append(str(point.lanePoint.paraPoint.laneId)) - lane_id.append(point.lanePoint.paraPoint.laneId) - para_offsets.append(point.lanePoint.paraPoint.parametricOffset) - - lane_id_selected, ok_pressed = QInputDialog.getItem( - QInputDialog(), - "Choose Lane ID", - "Lane ID", - tuple(lane_ids_to_match), - current=0, - editable=False) - - if ok_pressed: - i = lane_ids_to_match.index(lane_id_selected) - lane_id = lane_id[i] - para_offset = para_offsets[i] - parapoint = ad.map.point.createParaPoint(lane_id, para_offset) - lane_heading = ad.map.lane.getLaneENUHeading(parapoint) - return lane_heading - def spawn_pedestrian(self, enupoint, angle): """ Spawns pedestrian on the map and draws bounding boxes diff --git a/osc_generator/add_static_objects.py b/osc_generator/add_static_objects.py index 572b538..4a4c293 100644 --- a/osc_generator/add_static_objects.py +++ b/osc_generator/add_static_objects.py @@ -11,17 +11,18 @@ import math # pylint: disable=no-name-in-module, no-member from qgis.PyQt import QtWidgets, uic -from qgis.PyQt.QtCore import Qt, pyqtSignal, QVariant +from qgis.PyQt.QtCore import Qt, pyqtSignal from qgis.gui import QgsMapTool from qgis.utils import iface -from qgis.core import (QgsProject, QgsVectorLayer, QgsMessageLog, Qgis, QgsField, - QgsFeature, QgsGeometry, QgsPointXY, QgsPalLayerSettings, QgsVectorLayerSimpleLabeling, - QgsFeatureRequest) +from qgis.core import (QgsProject, Qgis, QgsFeature, QgsGeometry, QgsPointXY, + QgsFeatureRequest, QgsSpatialIndex) from PyQt5.QtWidgets import QInputDialog +from .helper_functions import layer_setup_props, display_message, is_float, verify_parameters # AD Map plugin import ad_map_access as ad + FORM_CLASS, _ = uic.loadUiType(os.path.join( os.path.dirname(__file__), 'add_static_objects_widget.ui')) @@ -43,42 +44,7 @@ def __init__(self, parent=None): self.props_labels_button.pressed.connect(self.toggle_labels) self._labels_on = True - self._props_layer = None - self.layer_setup() - - def layer_setup(self): - """ - Sets up layer for pedestrians - """ - root_layer = QgsProject.instance().layerTreeRoot() - osc_layer = root_layer.findGroup("OpenSCENARIO") - if not QgsProject.instance().mapLayersByName("Static Objects"): - props_layer = QgsVectorLayer("Polygon", "Static Objects", "memory") - QgsProject.instance().addMapLayer(props_layer, False) - osc_layer.addLayer(props_layer) - # Setup layer attributes - data_attributes = [QgsField("id", QVariant.Int), - QgsField("Prop", QVariant.String), - QgsField("Prop Type", QVariant.String), - QgsField("Orientation", QVariant.Double), - QgsField("Mass", QVariant.String), - QgsField("Pos X", QVariant.Double), - QgsField("Pos Y", QVariant.Double), - QgsField("Physics", QVariant.Bool)] - data_input = props_layer.dataProvider() - data_input.addAttributes(data_attributes) - props_layer.updateFields() - - label_settings = QgsPalLayerSettings() - label_settings.isExpression = True - label_settings.fieldName = "concat('Prop_', \"id\")" - props_layer.setLabeling(QgsVectorLayerSimpleLabeling(label_settings)) - props_layer.setLabelsEnabled(True) - - message = "Static objects layer added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - + layer_setup_props() self._props_layer = QgsProject.instance().mapLayersByName("Static Objects")[0] def toggle_labels(self): @@ -109,8 +75,7 @@ def insert_props(self): # UI Information message = "Using existing static objects layer" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) + display_message(message, level="Info") # Set map tool to point tool canvas = iface.mapCanvas() @@ -121,30 +86,28 @@ def insert_props(self): if self.props_orientation_use_lane.isChecked(): orientation = None else: - if self.is_float(self.props_orientation.text()): + if is_float(self.props_orientation.text()): orientation = float(self.props_orientation.text()) orientation = math.radians(orientation) else: - verification = self.verify_parameters(param=self.props_orientation.text()) + verification = verify_parameters(param=self.props_orientation.text()) if len(verification) == 0: # UI Information message = f"Parameter {self.props_orientation.text()} does not exist!" - iface.messageBar().pushMessage("Info", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: orientation = float(verification["Value"]) orientation = math.radians(orientation) mass = None - if self.is_float(self.props_mass.text()): + if is_float(self.props_mass.text()): mass = float(self.props_mass.text()) else: - verification = self.verify_parameters(param=self.props_mass.text()) + verification = verify_parameters(param=self.props_mass.text()) if len(verification) == 0: # UI Information message = f"Parameter {self.props_mass.text()} does not exist!" - iface.messageBar().pushMessage("Info", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") else: mass = self.props_mass.text() @@ -165,44 +128,6 @@ def override_orientation(self): else: self.props_orientation.setEnabled(True) - def verify_parameters(self, param): - """ - Checks Parameter Declarations attribute table to verify parameter exists - - Args: - param (string): name of parameter to check against - - Returns: - feature (dict): parameter definitions - """ - param_layer = QgsProject.instance().mapLayersByName("Parameter Declarations")[0] - query = f'"Parameter Name" = \'{param}\'' - feature_request = QgsFeatureRequest().setFilterExpression(query) - features = param_layer.getFeatures(feature_request) - feature = {} - - for feat in features: - feature["Type"] = feat["Type"] - feature["Value"] = feat["Value"] - - return feature - - def is_float(self, value): - """ - Checks value if it can be converted to float. - - Args: - value (string): value to check if can be converted to float - - Returns: - bool: True if float, False if not - """ - try: - float(value) - return True - except ValueError: - return False - #pylint: disable=missing-function-docstring class PointTool(QgsMapTool): @@ -230,8 +155,39 @@ def canvasReleaseEvent(self, event): point = self._canvas.getCoordinateTransform().toMapCoordinates(x, y) + lane_edge_layer = QgsProject.instance().mapLayersByName("Lane Edge")[0] + lane_edge_data_provider = lane_edge_layer.dataProvider() + spatial_index = QgsSpatialIndex() + spatial_feature = QgsFeature() + lane_edge_features = lane_edge_data_provider.getFeatures() + + while lane_edge_features.nextFeature(spatial_feature): + spatial_index.insertFeature(spatial_feature) + + nearest_ids = spatial_index.nearestNeighbor(point, 5) + + z_values = set() + for feat in lane_edge_layer.getFeatures(QgsFeatureRequest().setFilterFids(nearest_ids)): + feature_coordinates = feat.geometry().vertexAt(1) + z_values.add(round(feature_coordinates.z(), ndigits=4)) + + if max(z_values) - min(z_values) < 0.1: + altitude = max(z_values) + else: + stringified_z_values = [str(z_value) for z_value in z_values] + z_value_selected, ok_pressed = QInputDialog.getItem( + QInputDialog(), + "Choose Elevation", + "Elevation (meters)", + tuple(stringified_z_values), + current=0, + editable=False) + + if ok_pressed: + altitude = float(z_value_selected) + # Converting to ENU points - geopoint = ad.map.point.createGeoPoint(longitude=point.x(), latitude=point.y(), altitude=0) + geopoint = ad.map.point.createGeoPoint(longitude=point.x(), latitude=point.y(), altitude=altitude) enupoint = ad.map.point.toENU(geopoint) add_props = AddPropAttribute() @@ -256,6 +212,7 @@ def canvasReleaseEvent(self, event): prop_attr["Mass"], float(enupoint.x), float(enupoint.y), + float(enupoint.z) + 0.2, # Avoid ground collision prop_attr["Physics"]]) feature.setGeometry(QgsGeometry.fromPolygonXY([polygon_points])) self._data_input.addFeature(feature) @@ -293,7 +250,7 @@ def get_prop_heading(self, geopoint): Args: geopoint: [AD Map GEOPoint] point of click event """ - dist = ad.physics.Distance(0.025) + dist = ad.physics.Distance(1) admap_matched_points = ad.map.match.AdMapMatching.findLanes(geopoint, dist) lanes_detected = 0 @@ -302,8 +259,7 @@ def get_prop_heading(self, geopoint): if lanes_detected == 0: message = "Click point is too far from valid lane" - iface.messageBar().pushMessage("Error", message, level=Qgis.Critical) - QgsMessageLog.logMessage(message, level=Qgis.Critical) + display_message(message, level="Critical") return None elif lanes_detected == 1: for point in admap_matched_points: diff --git a/osc_generator/add_vehicles.py b/osc_generator/add_vehicles.py index f1b5ca8..d5ce613 100644 --- a/osc_generator/add_vehicles.py +++ b/osc_generator/add_vehicles.py @@ -5,21 +5,22 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . """ -OpenSCENARIO Generator - Add Static Objects +OpenSCENARIO Generator - Add Vehicles """ import math import os # pylint: disable=no-name-in-module, no-member -import ad_map_access as ad from PyQt5.QtWidgets import QInputDialog -from qgis.core import (Qgis, QgsFeature, QgsField, QgsGeometry, QgsMessageLog, QgsPointXY, - QgsProject, QgsVectorLayer, QgsPalLayerSettings, QgsVectorLayerSimpleLabeling, - QgsFeatureRequest) +from qgis.core import (Qgis, QgsFeature, QgsGeometry, QgsMessageLog, QgsPointXY, + QgsProject, QgsFeatureRequest, QgsSpatialIndex) from qgis.gui import QgsMapTool from qgis.PyQt import QtWidgets, uic -from qgis.PyQt.QtCore import Qt, QVariant, pyqtSignal +from qgis.PyQt.QtCore import Qt, pyqtSignal from qgis.utils import iface +from .helper_functions import layer_setup_vehicle + +import ad_map_access as ad FORM_CLASS, _ = uic.loadUiType(os.path.join( os.path.dirname(__file__), 'add_vehicles_widget.ui')) @@ -45,52 +46,7 @@ def __init__(self, parent=None): self._labels_on = True self._vehicle_layer_ego = None self._vehicle_layer = None - self.layer_setup() - - def layer_setup(self): - """ - Sets up layer for vehicles - """ - root_layer = QgsProject.instance().layerTreeRoot() - osc_layer = root_layer.findGroup("OpenSCENARIO") - if (not QgsProject.instance().mapLayersByName("Vehicles") or - not QgsProject.instance().mapLayersByName("Vehicles - Ego")): - vehicle_layer_ego = QgsVectorLayer("Polygon", "Vehicles - Ego", "memory") - vehicle_layer = QgsVectorLayer("Polygon", "Vehicles", "memory") - QgsProject.instance().addMapLayer(vehicle_layer_ego, False) - QgsProject.instance().addMapLayer(vehicle_layer, False) - osc_layer.addLayer(vehicle_layer_ego) - osc_layer.addLayer(vehicle_layer) - # Setup layer attributes - data_attributes = [QgsField("id", QVariant.Int), - QgsField("Vehicle Model", QVariant.String), - QgsField("Orientation", QVariant.Double), - QgsField("Pos X", QVariant.Double), - QgsField("Pos Y", QVariant.Double), - QgsField("Init Speed", QVariant.String), - QgsField("Agent", QVariant.String), - QgsField("Agent Camera", QVariant.Bool), - QgsField("Agent User Defined", QVariant.String)] - data_input_ego = vehicle_layer_ego.dataProvider() - data_input = vehicle_layer.dataProvider() - data_input_ego.addAttributes(data_attributes) - data_input.addAttributes(data_attributes) - vehicle_layer_ego.updateFields() - vehicle_layer.updateFields() - - label_settings_ego = QgsPalLayerSettings() - label_settings_ego.isExpression = True - label_settings_ego.fieldName = "concat('Ego_', \"id\")" - vehicle_layer_ego.setLabeling(QgsVectorLayerSimpleLabeling(label_settings_ego)) - vehicle_layer_ego.setLabelsEnabled(True) - label_settings = QgsPalLayerSettings() - label_settings.isExpression = True - label_settings.fieldName = "concat('Vehicle_', \"id\")" - vehicle_layer.setLabeling(QgsVectorLayerSimpleLabeling(label_settings)) - vehicle_layer.setLabelsEnabled(True) - - iface.messageBar().pushMessage("Info", "Vehicle layer added", level=Qgis.Info) - QgsMessageLog.logMessage("Vehicle layer added", level=Qgis.Info) + layer_setup_vehicle() self._vehicle_layer_ego = QgsProject.instance().mapLayersByName("Vehicles - Ego")[0] self._vehicle_layer = QgsProject.instance().mapLayersByName("Vehicles")[0] @@ -174,12 +130,16 @@ def insert_vehicle(self): canvas = iface.mapCanvas() layer = iface.mapCanvas().currentLayer() + if self.agent_selection.currentText() == "User Defined": + agent = self.agent_user_defined.text() + else: + agent = self.agent_selection.currentText() + vehicle_attributes = {"Model":self.vehicle_selection.currentText(), - "Orientation":orientation, - "InitSpeed":init_speed, - "Agent": self.agent_selection.currentText(), - "Agent Camera": self.agent_attach_camera.isChecked(), - "Agent User": self.agent_user_defined.text()} + "Orientation":orientation, + "InitSpeed":init_speed, + "Agent": agent, + "Agent Camera": self.agent_attach_camera.isChecked()} tool = PointTool(canvas, layer, vehicle_attributes) canvas.setMapTool(tool) @@ -277,8 +237,39 @@ def canvasReleaseEvent(self, event): point = self._canvas.getCoordinateTransform().toMapCoordinates(x, y) + lane_edge_layer = QgsProject.instance().mapLayersByName("Lane Edge")[0] + lane_edge_data_provider = lane_edge_layer.dataProvider() + spatial_index = QgsSpatialIndex() + spatial_feature = QgsFeature() + lane_edge_features = lane_edge_data_provider.getFeatures() + + while lane_edge_features.nextFeature(spatial_feature): + spatial_index.addFeature(spatial_feature) + + nearest_ids = spatial_index.nearestNeighbor(point, 5) + + z_values = set() + for feat in lane_edge_layer.getFeatures(QgsFeatureRequest().setFilterFids(nearest_ids)): + feature_coordinates = feat.geometry().vertexAt(1) + z_values.add(round(feature_coordinates.z(), ndigits=4)) + + if max(z_values) - min(z_values) < 0.1: + altitude = max(z_values) + else: + stringified_z_values = [str(z_value) for z_value in z_values] + z_value_selected, ok_pressed = QInputDialog.getItem( + QInputDialog(), + "Choose Elevation", + "Elevation (meters)", + tuple(stringified_z_values), + current=0, + editable=False) + + if ok_pressed: + altitude = float(z_value_selected) + # Converting to ENU points - geopoint = ad.map.point.createGeoPoint(longitude=point.x(), latitude=point.y(), altitude=0) + geopoint = ad.map.point.createGeoPoint(longitude=point.x(), latitude=point.y(), altitude=altitude) enupoint = ad.map.point.toENU(geopoint) add_veh = AddVehicleAttribute() @@ -294,15 +285,17 @@ def canvasReleaseEvent(self, event): # Set vehicle attributes feature = QgsFeature() - feature.setAttributes([veh_attr["id"], - veh_attr["Model"], - veh_attr["Orientation"], - float(enupoint.x), - float(enupoint.y), - veh_attr["InitSpeed"], - veh_attr["Agent"], - veh_attr["Agent Camera"], - veh_attr["Agent User"]]) + feature.setAttributes([ + veh_attr["id"], + veh_attr["Model"], + veh_attr["Orientation"], + float(enupoint.x), + float(enupoint.y), + float(enupoint.z) + 0.2, # Avoid ground collision + veh_attr["InitSpeed"], + veh_attr["Agent"], + veh_attr["Agent Camera"], + ]) feature.setGeometry(QgsGeometry.fromPolygonXY([polygon_points])) self._data_input.addFeature(feature) @@ -340,7 +333,7 @@ def get_vehicle_heading(self, geopoint): Args: geopoint: [AD Map GEOPoint] point of click event """ - dist = ad.physics.Distance(0.025) + dist = ad.physics.Distance(1) admap_matched_points = ad.map.match.AdMapMatching.findLanes(geopoint, dist) lanes_detected = 0 @@ -392,7 +385,6 @@ def spawnVehicle(self, enupoint, angle): enupoint: [AD Map ENUPoint] point of click event, as spawn center angle: [float] angle to rotate object (in radians) """ - if angle is not None: bot_left_x = float(enupoint.x) + (-2 * math.cos(angle) - 1 * math.sin(angle)) bot_left_y = float(enupoint.y) + (-2 * math.sin(angle) + 1 * math.cos(angle)) @@ -482,6 +474,5 @@ def get_vehicle_attributes(self, layer, attributes): "Orientation": orientation, "InitSpeed": attributes["InitSpeed"], "Agent": attributes["Agent"], - "Agent Camera": attributes["Agent Camera"], - "Agent User": attributes["Agent User"]} + "Agent Camera": attributes["Agent Camera"]} return vehicle_attributes diff --git a/osc_generator/edit_environment.py b/osc_generator/edit_environment.py index dc3a8ef..d95fbde 100644 --- a/osc_generator/edit_environment.py +++ b/osc_generator/edit_environment.py @@ -9,11 +9,11 @@ """ import os # pylint: disable=no-name-in-module, no-member -from qgis.core import (Qgis, QgsFeature, QgsField, QgsMessageLog, - QgsProject, QgsVectorLayer) +from qgis.core import Qgis, QgsFeature, QgsProject from qgis.PyQt import QtWidgets, uic -from qgis.PyQt.QtCore import QVariant, pyqtSignal +from qgis.PyQt.QtCore import pyqtSignal from qgis.utils import iface +from .helper_functions import layer_setup_environment, display_message FORM_CLASS, _ = uic.loadUiType(os.path.join( os.path.dirname(__file__), 'edit_environment_widget.ui')) @@ -37,37 +37,9 @@ def __init__(self, parent=None): self._layer = None self._data_provider = None - self.layer_setup() + layer_setup_environment() self.set_layer() - def layer_setup(self): - """ - Sets up layer for environment - """ - root_layer = QgsProject.instance().layerTreeRoot() - osc_layer = root_layer.findGroup("OpenSCENARIO") - if not QgsProject.instance().mapLayersByName("Environment"): - env_layer = QgsVectorLayer("None", "Environment", "memory") - QgsProject.instance().addMapLayer(env_layer, False) - osc_layer.addLayer(env_layer) - # Setup layer attributes - data_attributes = [QgsField("Datetime", QVariant.String), - QgsField("Datetime Animation", QVariant.Bool), - QgsField("Cloud State", QVariant.String), - QgsField("Fog Visual Range", QVariant.Double), - QgsField("Sun Intensity", QVariant.Double), - QgsField("Sun Azimuth", QVariant.Double), - QgsField("Sun Elevation", QVariant.Double), - QgsField("Precipitation Type", QVariant.String), - QgsField("Precipitation Intensity", QVariant.Double)] - data_input = env_layer.dataProvider() - data_input.addAttributes(data_attributes) - env_layer.updateFields() - # UI Information - message = "Environment layer added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - def closeEvent(self, event): """QtDockWidget signals for closing""" self.closingPlugin.emit() @@ -112,3 +84,6 @@ def AddEnvironment(self): sun_intensity, sun_azimuth, sun_elevation, percip_type, percip_intensity]) self._data_provider.addFeature(feature) + + message = "Environment variables added!" + display_message(message, level="Info") diff --git a/osc_generator/end_eval_criteria.py b/osc_generator/end_eval_criteria.py index 90d2391..705f2be 100644 --- a/osc_generator/end_eval_criteria.py +++ b/osc_generator/end_eval_criteria.py @@ -10,10 +10,11 @@ import os.path # pylint: disable=no-name-in-module, no-member -from qgis.core import Qgis, QgsFeature, QgsField, QgsMessageLog, QgsProject, QgsVectorLayer +from qgis.core import Qgis, QgsFeature, QgsProject from qgis.PyQt import QtWidgets, uic -from qgis.PyQt.QtCore import QVariant from qgis.utils import iface +from .helper_functions import layer_setup_end_eval + FORM_CLASS, _ = uic.loadUiType(os.path.join( os.path.dirname(__file__), 'end_eval_criteria_dialog.ui')) @@ -58,7 +59,7 @@ def default_triggers(self): def save_end_eval_kpis(self): """Executes ingestion of dialog form data into QGIS layer""" - self.layer_setup() + layer_setup_end_eval() layer = QgsProject.instance().mapLayersByName("End Evaluation KPIs")[0] self._data_provider = layer.dataProvider() # Clear existing attributes @@ -151,29 +152,6 @@ def get_wrong_lane(self): rule = self.wrongLane_Rule.currentText() self.write_attributes(cond_name, delay, cond_edge, param_ref, value, rule) - def layer_setup(self): - """Create no geometry layer in QGIS to save End Evaluation KPIs.""" - root_layer = QgsProject.instance().layerTreeRoot() - osc_layer = root_layer.findGroup("OpenSCENARIO") - if not QgsProject.instance().mapLayersByName("End Evaluation KPIs"): - env_eval_layer = QgsVectorLayer("None", "End Evaluation KPIs", "memory") - QgsProject.instance().addMapLayer(env_eval_layer, False) - osc_layer.addLayer(env_eval_layer) - # Setup layer attributes - data_attributes = [QgsField("Condition Name", QVariant.String), - QgsField("Delay", QVariant.Double), - QgsField("Condition Edge", QVariant.String), - QgsField("Parameter Ref", QVariant.String), - QgsField("Value", QVariant.Double), - QgsField("Rule", QVariant.String)] - self._data_provider = env_eval_layer.dataProvider() - self._data_provider.addAttributes(data_attributes) - env_eval_layer.updateFields() - # UI Information - message = "End evaluation KPIs layer added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - def write_attributes(self, cond_name, delay, cond_edge, param_ref, value, rule): """ Writes stop trigger attributes into QGIS attributes table. diff --git a/osc_generator/export_xosc.py b/osc_generator/export_xosc.py index be9f1f7..33f3841 100644 --- a/osc_generator/export_xosc.py +++ b/osc_generator/export_xosc.py @@ -31,6 +31,7 @@ def __init__(self, parent=None): # UI element signals self.select_path_button.pressed.connect(self.select_output) self.map_selection.currentTextChanged.connect(self.user_defined_map) + self.load_road_network() def select_output(self): """Prompts user to select output file""" @@ -64,6 +65,21 @@ def user_defined_map(self): else: self.map_selection_user_defined.setDisabled(True) + def load_road_network(self): + """Auto populates road network if available""" + if QgsProject.instance().mapLayersByName("Metadata"): + metadata_layer = QgsProject.instance().mapLayersByName("Metadata")[0] + for feature in metadata_layer.getFeatures(): + road_network = feature["Road Network"] + + index = self.map_selection.findText(road_network) + if index == -1: + self.map_selection.setCurrentText("User Defined") + self.map_selection_user_defined.setText(road_network) + else: + self.map_selection.setCurrentIndex(index) + + class GenerateXML(): """ Class for generating OpenSCENARIO files. @@ -335,6 +351,7 @@ def get_init(self, storyboard): orientation = feature["Orientation"] pos_x = feature["Pos X"] pos_y = feature["Pos Y"] + pos_z = feature["Pos Z"] init_speed = feature["Init Speed"] agent = feature["Agent"] agent_camera = str(feature["Agent Camera"]).lower() @@ -344,7 +361,7 @@ def get_init(self, storyboard): entity = etree.SubElement(init_act, "Private") entity.set("entityRef", str(veh_id)) - self.entity_teleport_action(entity, orientation, pos_x, pos_y) + self.entity_teleport_action(entity, orientation, pos_x, pos_y, pos_z) self.vehicle_controller(entity, str(feature["id"]), agent, agent_camera, is_ego=True) if init_speed != 0: self.set_init_speed(entity, init_speed) @@ -357,6 +374,7 @@ def get_init(self, storyboard): orientation = feature["Orientation"] pos_x = feature["Pos X"] pos_y = feature["Pos Y"] + pos_z = feature["Pos Z"] init_speed = feature["Init Speed"] agent = feature["Agent"] agent_camera = str(feature["Agent Camera"]).lower() @@ -366,7 +384,7 @@ def get_init(self, storyboard): entity = etree.SubElement(init_act, "Private") entity.set("entityRef", str(veh_id)) - self.entity_teleport_action(entity, orientation, pos_x, pos_y) + self.entity_teleport_action(entity, orientation, pos_x, pos_y, pos_z) self.vehicle_controller(entity, str(feature["id"]), agent, agent_camera, is_ego=False) if init_speed != 0: self.set_init_speed(entity, init_speed) @@ -379,11 +397,12 @@ def get_init(self, storyboard): orientation = feature["Orientation"] pos_x = feature["Pos X"] pos_y = feature["Pos Y"] + pos_z = feature["Pos Z"] init_speed = feature["Init Speed"] entity = etree.SubElement(init_act, "Private") entity.set("entityRef", ped_id) - self.entity_teleport_action(entity, orientation, pos_x, pos_y) + self.entity_teleport_action(entity, orientation, pos_x, pos_y, pos_z) if init_speed != 0: self.set_init_speed(entity, init_speed) @@ -395,12 +414,13 @@ def get_init(self, storyboard): orientation = feature["Orientation"] pos_x = feature["Pos X"] pos_y = feature["Pos Y"] + pos_z = feature["Pos Z"] entity = etree.SubElement(init_act, "Private") entity.set("entityRef", prop_id) - self.entity_teleport_action(entity, orientation, pos_x, pos_y) + self.entity_teleport_action(entity, orientation, pos_x, pos_y, pos_z) - def entity_teleport_action(self, entity, orientation, pos_x, pos_y): + def entity_teleport_action(self, entity, orientation, pos_x, pos_y, pos_z): """ Writes OpenSCENARIO tags for entity teleport action @@ -409,6 +429,7 @@ def entity_teleport_action(self, entity, orientation, pos_x, pos_y): orientation: [double] orientation of entity pos_x: [double] position x of entity (in meters) pos_y: [double] position y of entity (in meters) + pos_z: [double] position z of entity (in meters) """ private_act = etree.SubElement(entity, "PrivateAction") teleport_action = etree.SubElement(private_act, "TeleportAction") @@ -416,7 +437,7 @@ def entity_teleport_action(self, entity, orientation, pos_x, pos_y): teleport_worldpos = etree.SubElement(teleport_pos, "WorldPosition") teleport_worldpos.set("x", str(pos_x)) teleport_worldpos.set("y", str(pos_y)) - teleport_worldpos.set("z", "0.2") + teleport_worldpos.set("z", str(pos_z)) teleport_worldpos.set("h", str(orientation)) def vehicle_controller(self, entity, veh_id, agent, agent_camera, is_ego): @@ -925,7 +946,7 @@ def get_maneuver_start_trigger(self, feature, event): world_position = etree.SubElement(position, "WorldPosition") world_position.set("x", str(feature["Start - WorldPos: X"])) world_position.set("y", str(feature["Start - WorldPos: Y"])) - world_position.set("z", "0") + world_position.set("z", str(feature["Start - WorldPos: Z"])) world_position.set("h", str(feature["Start - WorldPos: Heading"])) elif feature["Start Trigger"] == "by Value": @@ -1025,7 +1046,7 @@ def get_maneuver_stop_trigger(self, feature, event): world_position = etree.SubElement(position, "WorldPosition") world_position.set("x", str(feature["Stop - WorldPos: X"])) world_position.set("y", str(feature["Stop - WorldPos: Y"])) - world_position.set("z", "0") + world_position.set("z", str(feature["Stop - WorldPos: Z"])) world_position.set("h", str(feature["Stop - WorldPos: Heading"])) elif feature["Stop Trigger"] == "by Value": @@ -1062,7 +1083,6 @@ def get_maneuver_stop_trigger(self, feature, event): value_cond_element.set("trafficSignalControllerRef", feature["Value: TController Ref"]) value_cond_element.set("phase", feature["Stop - Value: TController Phase"]) - def get_story_start_trigger(self, act): """ Writes story start triggers. @@ -1147,20 +1167,15 @@ def write_xosc(self, generated_xml): xosc_file.write(reparsed_xml) xosc_file.close() - text = f"Successfully exported OpenSCENARIO file to {self._filepath}" msg = QMessageBox() - msg.setIcon(QMessageBox.Information) + if self._warning_message: + msg.setIcon(QMessageBox.Warning) + text = f"Exported OpenSCENARIO file {self._filepath} has warnings!\n\n" + text += "\n".join(self._warning_message) + else: + msg.setIcon(QMessageBox.Information) + text = f"Successfully exported OpenSCENARIO file to {self._filepath}" msg.setText(text) msg.setWindowTitle("OpenSCENARIO Export") msg.setStandardButtons(QMessageBox.Ok) msg.exec() - - if self._warning_message: - warn_msg = QMessageBox() - warn_msg.setIcon(QMessageBox.Warning) - warn_msg_text = "Exported OpenSCENARIO file has warnings!\n\n" - warn_msg_text += "\n".join(self._warning_message) - warn_msg.setText(warn_msg_text) - warn_msg.setWindowTitle("OpenSCENARIO Export Warnings") - warn_msg.setStandardButtons(QMessageBox.Ok) - warn_msg.exec() diff --git a/osc_generator/helper_functions.py b/osc_generator/helper_functions.py new file mode 100644 index 0000000..8f337de --- /dev/null +++ b/osc_generator/helper_functions.py @@ -0,0 +1,574 @@ +# -*- coding: utf-8 -*- + +# Copyright (c) 2020-2021 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +""" +OpenSCENARIO Generator - Helper Functions + +A collection of helper functions used throughout the plugin +""" +from PyQt5.QtWidgets import QInputDialog +from qgis.core import (Qgis, QgsProject, QgsMessageLog, QgsVectorLayer, QgsFeature, + QgsField, QgsPalLayerSettings, QgsVectorLayerSimpleLabeling, QgsFeatureRequest) +from qgis.utils import iface +from qgis.PyQt.QtCore import QVariant + +import ad_map_access as ad + +def display_message(message, level): + """ + Presents status messages on UI + + Args: + message (str): Status message to display + level (str): 3 levels -> Info, Warning, Critical + """ + status = level + + # Convert into QGIS message levels + if level is "Info": + level = Qgis.Info + elif level is "Warning": + level = Qgis.Warning + elif level is "Critical": + level = Qgis.Critical + + iface.messageBar().pushMessage(status, message, level=level) + QgsMessageLog.logMessage(message, level=level) + +def layer_setup_metadata(): + """ + Set up OpenSCENARIO metadata layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("Metadata"): + metadata_layer = QgsVectorLayer("None", "Metadata", "memory") + QgsProject.instance().addMapLayer(metadata_layer, False) + osc_layer.addLayer(metadata_layer) + + # Setup layer attributes + data_attibutes = [ + QgsField("Rev Major", QVariant.Int), + QgsField("Rev Minor", QVariant.Int), + QgsField("Description", QVariant.String), + QgsField("Author", QVariant.String), + QgsField("Road Network", QVariant.String), + QgsField("Scene Graph File", QVariant.String) + ] + metadata_layer.dataProvider().addAttributes(data_attibutes) + metadata_layer.updateFields() + + message = "Metadata layer added" + display_message(message, level="Info") + +def layer_setup_end_eval(): + """ + Set up OpenSCENARIO end evaluation KPIs layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("End Evaluation KPIs"): + end_eval_layer = QgsVectorLayer("None", "End Evaluation KPIs", "memory") + QgsProject.instance().addMapLayer(end_eval_layer, False) + osc_layer.addLayer(end_eval_layer) + # Setup layer attributes + data_attributes = [ + QgsField("Condition Name", QVariant.String), + QgsField("Delay", QVariant.Double), + QgsField("Condition Edge", QVariant.String), + QgsField("Parameter Ref", QVariant.String), + QgsField("Value", QVariant.Double), + QgsField("Rule", QVariant.String) + ] + + end_eval_layer.dataProvider().addAttributes(data_attributes) + end_eval_layer.updateFields() + + message = "End evaluation KPIs layer added" + display_message(message, level="Info") + +def layer_setup_environment(): + """ + Set up environment layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("Environment"): + env_layer = QgsVectorLayer("None", "Environment", "memory") + QgsProject.instance().addMapLayer(env_layer, False) + osc_layer.addLayer(env_layer) + + # Setup layer attributes + data_attributes = [ + QgsField("Datetime", QVariant.String), + QgsField("Datetime Animation", QVariant.Bool), + QgsField("Cloud State", QVariant.String), + QgsField("Fog Visual Range", QVariant.Double), + QgsField("Sun Intensity", QVariant.Double), + QgsField("Sun Azimuth", QVariant.Double), + QgsField("Sun Elevation", QVariant.Double), + QgsField("Precipitation Type", QVariant.String), + QgsField("Precipitation Intensity", QVariant.Double) + ] + env_layer.dataProvider().addAttributes(data_attributes) + env_layer.updateFields() + + message = "Environment layer added" + display_message(message, level="Info") + +def layer_setup_walker(): + """ + Set up pedestrian layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("Pedestrians"): + walker_layer = QgsVectorLayer("Polygon", "Pedestrians", "memory") + QgsProject.instance().addMapLayer(walker_layer, False) + osc_layer.addLayer(walker_layer) + + # Setup layer attributes + data_attributes = [ + QgsField("id", QVariant.Int), + QgsField("Walker", QVariant.String), + QgsField("Orientation", QVariant.Double), + QgsField("Pos X", QVariant.Double), + QgsField("Pos Y", QVariant.Double), + QgsField("Pos Z", QVariant.Double), + QgsField("Init Speed", QVariant.String) + ] + walker_layer.dataProvider().addAttributes(data_attributes) + walker_layer.updateFields() + + label_settings = QgsPalLayerSettings() + label_settings.isExpression = True + label_settings.fieldName = "concat('Pedestrian_', \"id\")" + walker_layer.setLabeling(QgsVectorLayerSimpleLabeling(label_settings)) + walker_layer.setLabelsEnabled(True) + + message = "Pedestrian layer added" + display_message(message, level="Info") + +def layer_setup_vehicle(): + """ + Set up vehicle layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if (not QgsProject.instance().mapLayersByName("Vehicles") or + not QgsProject.instance().mapLayersByName("Vehicles - Ego")): + vehicle_layer_ego = QgsVectorLayer("Polygon", "Vehicles - Ego", "memory") + vehicle_layer = QgsVectorLayer("Polygon", "Vehicles", "memory") + QgsProject.instance().addMapLayer(vehicle_layer_ego, False) + QgsProject.instance().addMapLayer(vehicle_layer, False) + osc_layer.addLayer(vehicle_layer_ego) + osc_layer.addLayer(vehicle_layer) + + # Setup layer attributes + data_attributes = [QgsField("id", QVariant.Int), + QgsField("Vehicle Model", QVariant.String), + QgsField("Orientation", QVariant.Double), + QgsField("Pos X", QVariant.Double), + QgsField("Pos Y", QVariant.Double), + QgsField("Pos Z", QVariant.Double), + QgsField("Init Speed", QVariant.String), + QgsField("Agent", QVariant.String), + QgsField("Agent Camera", QVariant.Bool)] + + vehicle_layer_ego.dataProvider().addAttributes(data_attributes) + vehicle_layer.dataProvider().addAttributes(data_attributes) + vehicle_layer_ego.updateFields() + vehicle_layer.updateFields() + + label_settings_ego = QgsPalLayerSettings() + label_settings_ego.isExpression = True + label_settings_ego.fieldName = "concat('Ego_', \"id\")" + vehicle_layer_ego.setLabeling(QgsVectorLayerSimpleLabeling(label_settings_ego)) + vehicle_layer_ego.setLabelsEnabled(True) + label_settings = QgsPalLayerSettings() + label_settings.isExpression = True + label_settings.fieldName = "concat('Vehicle_', \"id\")" + vehicle_layer.setLabeling(QgsVectorLayerSimpleLabeling(label_settings)) + vehicle_layer.setLabelsEnabled(True) + + message = "Vehicle layer added" + display_message(message, level="Info") + +def layer_setup_props(): + """ + Set up static objects layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("Static Objects"): + props_layer = QgsVectorLayer("Polygon", "Static Objects", "memory") + QgsProject.instance().addMapLayer(props_layer, False) + osc_layer.addLayer(props_layer) + # Setup layer attributes + data_attributes = [ + QgsField("id", QVariant.Int), + QgsField("Prop", QVariant.String), + QgsField("Prop Type", QVariant.String), + QgsField("Orientation", QVariant.Double), + QgsField("Mass", QVariant.String), + QgsField("Pos X", QVariant.Double), + QgsField("Pos Y", QVariant.Double), + QgsField("Pos Z", QVariant.Double), + QgsField("Physics", QVariant.Bool) + ] + props_layer.dataProvider().addAttributes(data_attributes) + props_layer.updateFields() + + label_settings = QgsPalLayerSettings() + label_settings.isExpression = True + label_settings.fieldName = "concat('Prop_', \"id\")" + props_layer.setLabeling(QgsVectorLayerSimpleLabeling(label_settings)) + props_layer.setLabelsEnabled(True) + + message = "Static objects layer added" + display_message(message, level="Info") + +def layer_setup_maneuvers_waypoint(): + """ + Set up waypoint maneuvers layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("Waypoint Maneuvers"): + waypoint_layer = QgsVectorLayer("Point", "Waypoint Maneuvers", "memory") + QgsProject.instance().addMapLayer(waypoint_layer, False) + osc_layer.addLayer(waypoint_layer) + # Setup layer attributes + data_attributes = [ + QgsField("Maneuver ID", QVariant.Int), + QgsField("Entity", QVariant.String), + QgsField("Waypoint No", QVariant.Int), + QgsField("Orientation", QVariant.Double), + QgsField("Pos X", QVariant.Double), + QgsField("Pos Y", QVariant.Double), + QgsField("Pos Z", QVariant.Double), + QgsField("Route Strategy", QVariant.String) + ] + waypoint_layer.dataProvider().addAttributes(data_attributes) + waypoint_layer.updateFields() + + label_settings = QgsPalLayerSettings() + label_settings.isExpression = True + label_name = "concat('ManID: ', \"Maneuver ID\", ' ', \"Entity\", ' - ', \"Waypoint No\")" + label_settings.fieldName = label_name + waypoint_layer.setLabeling(QgsVectorLayerSimpleLabeling(label_settings)) + waypoint_layer.setLabelsEnabled(True) + + message = "Waypoint maneuvers layer added" + display_message(message, level="Info") + else: + message = "Using existing waypoint maneuver layer" + display_message(message, level="Info") + +def layer_setup_maneuvers_and_triggers(): + """ + Set up maneuvers layer (including start and stop triggers) + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("Maneuvers"): + maneuver_layer = QgsVectorLayer("None", "Maneuvers", "memory") + QgsProject.instance().addMapLayer(maneuver_layer, False) + osc_layer.addLayer(maneuver_layer) + # Setup layer attributes + data_attributes = [ + QgsField("id", QVariant.Int), + QgsField("Maneuver Type", QVariant.String), + QgsField("Entity", QVariant.String), + QgsField("Entity: Maneuver Type", QVariant.String), + # Global Actions + QgsField("Global: Act Type", QVariant.String), + QgsField("Infra: Traffic Light ID", QVariant.Int), + QgsField("Infra: Traffic Light State", QVariant.String), + # Start Triggers + QgsField("Start Trigger", QVariant.String), + QgsField("Start - Entity: Condition", QVariant.String), + QgsField("Start - Entity: Ref Entity", QVariant.String), + QgsField("Start - Entity: Duration", QVariant.Double), + QgsField("Start - Entity: Value", QVariant.Double), + QgsField("Start - Entity: Rule", QVariant.String), + QgsField("Start - Entity: RelDistType", QVariant.String), + QgsField("Start - Entity: Freespace", QVariant.Bool), + QgsField("Start - Entity: Along Route", QVariant.Bool), + QgsField("Start - Value: Condition", QVariant.String), + QgsField("Start - Value: Param Ref", QVariant.String), + QgsField("Start - Value: Name", QVariant.String), + QgsField("Start - Value: DateTime", QVariant.String), + QgsField("Start - Value: Value", QVariant.Double), + QgsField("Start - Value: Rule", QVariant.String), + QgsField("Start - Value: State", QVariant.String), + QgsField("Start - Value: Sboard Type", QVariant.String), + QgsField("Start - Value: Sboard Element", QVariant.String), + QgsField("Start - Value: Sboard State", QVariant.String), + QgsField("Start - Value: TController Ref", QVariant.String), + QgsField("Start - Value: TController Phase", QVariant.String), + QgsField("Start - WorldPos: Tolerance", QVariant.Double), + QgsField("Start - WorldPos: X", QVariant.Double), + QgsField("Start - WorldPos: Y", QVariant.Double), + QgsField("Start - WorldPos: Z", QVariant.Double), + QgsField("Start - WorldPos: Heading", QVariant.Double), + # Stop Triggers + QgsField("Stop Trigger Enabled", QVariant.Bool), + QgsField("Stop Trigger", QVariant.String), + QgsField("Stop - Entity: Condition", QVariant.String), + QgsField("Stop - Entity: Ref Entity", QVariant.String), + QgsField("Stop - Entity: Duration", QVariant.Double), + QgsField("Stop - Entity: Value", QVariant.Double), + QgsField("Stop - Entity: Rule", QVariant.String), + QgsField("Stop - Entity: RelDistType", QVariant.String), + QgsField("Stop - Entity: Freespace", QVariant.Bool), + QgsField("Stop - Entity: Along Route", QVariant.Bool), + QgsField("Stop - Value: Condition", QVariant.String), + QgsField("Stop - Value: Param Ref", QVariant.String), + QgsField("Stop - Value: Name", QVariant.String), + QgsField("Stop - Value: DateTime", QVariant.String), + QgsField("Stop - Value: Value", QVariant.Double), + QgsField("Stop - Value: Rule", QVariant.String), + QgsField("Stop - Value: State", QVariant.String), + QgsField("Stop - Value: Sboard Type", QVariant.String), + QgsField("Stop - Value: Sboard Element", QVariant.String), + QgsField("Stop - Value: Sboard State", QVariant.String), + QgsField("Stop - Value: TController Ref", QVariant.String), + QgsField("Stop - Value: TController Phase", QVariant.String), + QgsField("Stop - WorldPos: Tolerance", QVariant.Double), + QgsField("Stop - WorldPos: X", QVariant.Double), + QgsField("Stop - WorldPos: Y", QVariant.Double), + QgsField("Stop - WorldPos: Z", QVariant.Double), + QgsField("Stop - WorldPos: Heading", QVariant.Double) + ] + maneuver_layer.dataProvider().addAttributes(data_attributes) + maneuver_layer.updateFields() + + message = "Maneuvers layer added" + display_message(message, level="Info") + else: + message = "Using existing maneuvers layer" + display_message(message, level="Info") + +def layer_setup_maneuvers_longitudinal(): + """ + Set up longitudinal maneuvers layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("Longitudinal Maneuvers"): + long_man_layer = QgsVectorLayer("None", "Longitudinal Maneuvers", "memory") + QgsProject.instance().addMapLayer(long_man_layer, False) + osc_layer.addLayer(long_man_layer) + # Setup layer attributes + data_attributes = [ + QgsField("Maneuver ID", QVariant.Int), + QgsField("Type", QVariant.String), + QgsField("Speed Target", QVariant.String), + QgsField("Ref Entity", QVariant.String), + QgsField("Dynamics Shape", QVariant.String), + QgsField("Dynamics Dimension", QVariant.String), + QgsField("Dynamics Value", QVariant.String), + QgsField("Target Type", QVariant.String), + QgsField("Target Speed", QVariant.String), + QgsField("Continuous", QVariant.Bool), + QgsField("Freespace", QVariant.Bool), + QgsField("Max Acceleration", QVariant.String), + QgsField("Max Deceleration", QVariant.String), + QgsField("Max Speed", QVariant.String) + ] + long_man_layer.dataProvider().addAttributes(data_attributes) + long_man_layer.updateFields() + + message = "Longitudinal maneuvers layer added" + display_message(message, level="Info") + else: + message = "Using existing longitudinal maneuvers layer" + display_message(message, level="Info") + +def layer_setup_maneuvers_lateral(): + """ + Set up lateral maneuvers layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("Lateral Maneuvers"): + lat_man_layer = QgsVectorLayer("None", "Lateral Maneuvers", "memory") + QgsProject.instance().addMapLayer(lat_man_layer, False) + osc_layer.addLayer(lat_man_layer) + # Setup layer attributes + data_attributes = [ + QgsField("Maneuver ID", QVariant.Int), + QgsField("Type", QVariant.String), + QgsField("Lane Target", QVariant.String), + QgsField("Ref Entity", QVariant.String), + QgsField("Dynamics Shape", QVariant.String), + QgsField("Dynamics Dimension", QVariant.String), + QgsField("Dynamics Value", QVariant.String), + QgsField("Lane Target Value", QVariant.String), + QgsField("Max Lateral Acceleration", QVariant.String), + QgsField("Max Acceleration", QVariant.String), + QgsField("Max Deceleration", QVariant.String), + QgsField("Max Speed", QVariant.String) + ] + lat_man_layer.dataProvider().addAttributes(data_attributes) + lat_man_layer.updateFields() + + message = "Lateral maneuvers layer added" + display_message(message, level="Info") + else: + message = "Using existing lateral maneuvers layer" + display_message(message, level="Info") + +def layer_setup_parameters(): + """ + Set up parameter declarations layer + """ + root_layer = QgsProject.instance().layerTreeRoot() + osc_layer = root_layer.findGroup("OpenSCENARIO") + if osc_layer is None: + osc_layer = root_layer.addGroup("OpenSCENARIO") + + if not QgsProject.instance().mapLayersByName("Parameter Declarations"): + param_layer = QgsVectorLayer("None", "Parameter Declarations", "memory") + QgsProject.instance().addMapLayer(param_layer, False) + osc_layer.addLayer(param_layer) + # Setup layer attributes + data_attributes = [ + QgsField("Parameter Name", QVariant.String), + QgsField("Type", QVariant.String), + QgsField("Value", QVariant.String) + ] + param_layer.dataProvider().addAttributes(data_attributes) + param_layer.updateFields() + + message = "Parameter declarations layer added" + display_message(message, level="Info") + +def verify_parameters(param): + """ + Checks Parameter Declarations attribute table to verify parameter exists + + Args: + param (string): name of parameter to check against + + Returns: + feature (dict): parameter definitions + """ + param_layer = QgsProject.instance().mapLayersByName("Parameter Declarations")[0] + query = f'"Parameter Name" = \'{param}\'' + feature_request = QgsFeatureRequest().setFilterExpression(query) + features = param_layer.getFeatures(feature_request) + feature = {} + + for feat in features: + feature["Type"] = feat["Type"] + feature["Value"] = feat["Value"] + + return feature + +def is_float(value): + """ + Checks value if it can be converted to float. + + Args: + value (string): value to check if can be converted to float + + Returns: + bool: True if float, False if not + """ + try: + float(value) + return True + except ValueError: + return False + +def get_entity_heading(geopoint): + """ + Acquires heading based on spawn position in map. + Prompts user to select lane if multiple lanes exist at spawn position. + Throws error if spawn position is not on lane. + + Args: + geopoint: [AD Map GEOPoint] point of click event + + Returns: + lane_heading: [float] heading of click point at selected lane ID + lane_heading: [None] if click point is not valid + """ + dist = ad.physics.Distance(1) + admap_matched_points = ad.map.match.AdMapMatching.findLanes(geopoint, dist) + + lanes_detected = 0 + for point in admap_matched_points: + lanes_detected += 1 + + if lanes_detected == 0: + message = "Click point is too far from valid lane" + iface.messageBar().pushMessage("Error", message, level=Qgis.Critical) + QgsMessageLog.logMessage(message, level=Qgis.Critical) + return None + elif lanes_detected == 1: + for point in admap_matched_points: + lane_id = point.lanePoint.paraPoint.laneId + para_offset = point.lanePoint.paraPoint.parametricOffset + parapoint = ad.map.point.createParaPoint(lane_id, para_offset) + lane_heading = ad.map.lane.getLaneENUHeading(parapoint) + return lane_heading + else: + lane_ids_to_match = [] + lane_id = [] + para_offsets = [] + for point in admap_matched_points: + lane_ids_to_match.append(str(point.lanePoint.paraPoint.laneId)) + lane_id.append(point.lanePoint.paraPoint.laneId) + para_offsets.append(point.lanePoint.paraPoint.parametricOffset) + + lane_id_selected, ok_pressed = QInputDialog.getItem( + QInputDialog(), + "Choose Lane ID", + "Lane ID", + tuple(lane_ids_to_match), + current=0, + editable=False) + + if ok_pressed: + i = lane_ids_to_match.index(lane_id_selected) + lane_id = lane_id[i] + para_offset = para_offsets[i] + parapoint = ad.map.point.createParaPoint(lane_id, para_offset) + lane_heading = ad.map.lane.getLaneENUHeading(parapoint) + return lane_heading diff --git a/osc_generator/import_xosc.py b/osc_generator/import_xosc.py new file mode 100644 index 0000000..0d7a84e --- /dev/null +++ b/osc_generator/import_xosc.py @@ -0,0 +1,1148 @@ +# -*- coding: utf-8 -*- + +# Copyright (c) 2020-2021 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +""" +OpenSCENARIO Generator - Import XOSC +""" +import os +import math +from qgis.PyQt import QtWidgets, uic +from PyQt5.QtWidgets import QFileDialog, QMessageBox +from qgis.core import QgsProject, QgsFeature, QgsPointXY, QgsGeometry +from defusedxml import ElementTree as etree +from .helper_functions import (layer_setup_environment, layer_setup_metadata, layer_setup_vehicle, + layer_setup_walker, layer_setup_props, layer_setup_end_eval, layer_setup_maneuvers_and_triggers, + layer_setup_maneuvers_lateral, layer_setup_maneuvers_longitudinal, layer_setup_maneuvers_waypoint, + layer_setup_parameters, is_float, display_message) +import ad_map_access as ad + +FORM_CLASS, _ = uic.loadUiType(os.path.join( + os.path.dirname(__file__), 'import_xosc_dialog.ui')) + +class ImportXOSCDialog(QtWidgets.QDialog, FORM_CLASS): + """ + Dialog class for importing OpenSCENARIO XML files. + """ + def __init__(self, parent=None): + """Initialization of ExportXMLDialog""" + super(ImportXOSCDialog, self).__init__(parent) + self.setupUi(self) + # UI element signals + self.import_path_button.pressed.connect(self.select_input) + + def select_input(self): + """Prompts user to select file to import""" + filename, _filter = QFileDialog.getOpenFileName( + self, + caption="Select OpenSCENARIO file to import", + filter="OpenSCENARIO (*.xosc)") + # Update text field only if user press 'OK' + if filename: + self.import_path.setText(filename) + + def open_file(self): + """Opens OpenSCENARIO file and start parsing into QGIS layers""" + if self.import_path.text() is not "": + filepath = self.import_path.text() + read_xosc = ImportXOSC(filepath) + read_xosc.import_xosc() + else: + message = "No file path was given for importing" + display_message(message, level="Critical") + + +class ImportXOSC(): + def __init__(self, filepath): + self._filepath = filepath + self._invert_y = False + self._warning_message = [] + + self.setup_qgis_layers() + + def setup_qgis_layers(self): + """ + Initiates layers in QGIS if they are not already created. + """ + layer_setup_environment() + layer_setup_metadata() + layer_setup_vehicle() + layer_setup_walker() + layer_setup_props() + layer_setup_end_eval() + layer_setup_maneuvers_and_triggers() + layer_setup_maneuvers_lateral() + layer_setup_maneuvers_longitudinal() + layer_setup_maneuvers_waypoint() + layer_setup_parameters() + + def import_xosc(self): + tree = etree.parse(self._filepath) + self._root = tree.getroot() + + self.parse_osc_metadata() + self.parse_paremeter_declarations() + + if self._root.findall(".//EnvironmentAction"): + env_node = self._root.findall(".//EnvironmentAction")[0] + self.parse_enviroment_actions(env_node) + else: + self._warning_message.append("No environment actions found") + + if self._root.findall(".//Entities"): + entity_node = self._root.findall(".//Entities")[0] + self.parse_entities(entity_node) + else: + self._warning_message.append("No entities found") + + if self._root.findall(".//Storyboard/StopTrigger/ConditionGroup"): + end_eval_node = self._root.findall(".//Storyboard/StopTrigger")[0] + self.parse_end_evals(end_eval_node) + else: + self._warning_message.append("No end evaluation KPIs found") + + if self._root.findall(".//Story"): + story_node = self._root.findall(".//Story")[0] + self.parse_maneuvers(story_node) + else: + self._warning_message.append("No maneuvers found") + + msg = QMessageBox() + if self._warning_message: + msg.setIcon(QMessageBox.Warning) + text = f"Imported OpenSCENARIO file {self._filepath} has warnings!\n\n" + text += "\n".join(self._warning_message) + else: + msg.setIcon(QMessageBox.Information) + text = f"Successfully imported OpenSCENARIO file {self._filepath}" + msg.setText(text) + msg.setWindowTitle("OpenSCENARIO Import") + msg.setStandardButtons(QMessageBox.Ok) + msg.exec() + + def parse_osc_metadata(self): + """ + Parses OpenSCENARIO Metadata (File Headers, Road Network, Scene Graph File) + """ + file_header_node = self._root.findall(".//FileHeader")[0] + rev_major = file_header_node.attrib.get("revMajor") + rev_minor = file_header_node.attrib.get("revMinor") + description = file_header_node.attrib.get("description") + if description[:6] == "CARLA:": + self._invert_y = True + description = description[6:] + author = file_header_node.attrib.get("author") + + logic_file = self._root.findall(".//RoadNetwork/LogicFile")[0] + road_network_filepath = logic_file.attrib.get("filepath") + scene_graph_file = self._root.findall(".//RoadNetwork/SceneGraphFile")[0] + scene_graph_filepath = scene_graph_file.attrib.get("filepath") + + if not QgsProject.instance().mapLayersByName("Metadata"): + layer_setup_metadata() + + metadata_layer = QgsProject.instance().mapLayersByName("Metadata")[0] + current_features = [feat.id() for feat in metadata_layer.getFeatures()] + metadata_layer.dataProvider().deleteFeatures(current_features) + + feature = QgsFeature() + feature.setAttributes([ + int(rev_major), + int(rev_minor), + description, + author, + road_network_filepath, + scene_graph_filepath + ]) + metadata_layer.dataProvider().addFeature(feature) + + def parse_paremeter_declarations(self): + """ + Parses parameter declarations + """ + param_layer = QgsProject.instance().mapLayersByName("Parameter Declarations")[0] + param_name = "" + param_type = "" + param_value = "" + + param_group_node = self._root.find(".//ParameterDeclarations") + for param_node in param_group_node.iter("ParameterDeclaration"): + param_name = param_node.attrib.get("name") + param_type = param_node.attrib.get("type") + param_value = param_node.attrib.get("value") + + feature = QgsFeature() + feature.setAttributes([ + param_name, + param_type, + param_value + ]) + param_layer.dataProvider().addFeature(feature) + + def parse_enviroment_actions(self, env_node): + """ + Parses environment information and saves into QGIS layer + + Args: + env_node (XML element) + """ + environment = env_node.findall("Environment") + for element in environment: + time_of_day = element.find("TimeOfDay") + weather = element.find("Weather") + sun = weather.find("Sun") + fog = weather.find("Fog") + precipitation = weather.find("Precipitation") + road_condition = element.find("RoadCondition") + + datetime = time_of_day.attrib.get("dateTime") + datatime_animation = time_of_day.attrib.get("animation") + + cloud = weather.attrib.get("cloudState") + fog_range = fog.attrib.get("visualRange") + sun_azimuth = sun.attrib.get("azimuth") + sun_elevation = sun.attrib.get("elevation") + sun_intensity = sun.attrib.get("intensity") + precip_intensity = precipitation.attrib.get("intensity") + precip_type = precipitation.attrib.get("precipitationType") + friction_scale_factor = road_condition.attrib.get("frictionScaleFactor") + + env_layer = QgsProject.instance().mapLayersByName("Environment")[0] + current_features = [feat.id() for feat in env_layer.getFeatures()] + env_layer.dataProvider().deleteFeatures(current_features) + + feature = QgsFeature() + feature.setAttributes([datetime, datatime_animation, + cloud, fog_range, + sun_intensity, sun_azimuth, sun_elevation, + precip_type, precip_intensity]) + env_layer.dataProvider().addFeature(feature) + + def parse_entities(self, entity_node): + """ + Parses entity information and saves into QGIS layers + + Args: + entity_node (XML element): Node that contains the entity + """ + parent_map = {c: p for p in entity_node.iter() for c in p} + for scenario_object in entity_node.iter("ScenarioObject"): + for pedestrian in scenario_object.iter("Pedestrian"): + parent = parent_map[pedestrian] + actor_name = parent.attrib.get("name") + self.parse_pedestrian(pedestrian, actor_name) + + for vehicle in scenario_object.iter("Vehicle"): + parent = parent_map[vehicle] + actor_name = parent.attrib.get("name") + self.parse_vehicle(vehicle, actor_name) + + for props in scenario_object.iter("MiscObject"): + parent = parent_map[props] + actor_name = parent.attrib.get("name") + self.parse_prop(props, actor_name) + + def parse_pedestrian(self, pedestrian, actor_name): + """ + Extracts information for pedestrian and inserts into QGIS layer + + Args: + pedestrian (XML element) + actor_name (string): actor name to match in Init and get positions + """ + # Query to get Init elements of same actor + query = f".//Private[@entityRef='{actor_name}']" + found_init = self._root.find(query) + + world_pos_x = 0 + world_pos_y = 0 + world_pos_z = 0 + world_pos_heading = 0 + world_pos = found_init.find(".//WorldPosition") + if world_pos is not None: + world_pos_x = world_pos.attrib.get("x") + world_pos_y = world_pos.attrib.get("y") + world_pos_z = world_pos.attrib.get("z") + world_pos_heading = world_pos.attrib.get("h") + else: + message = (f"Non WorldPosition waypoints are not supported (Entity: {actor_name})" + "Defaulting to WorldPos 0, 0, 0") + display_message(message, level="Info") + self._warning_message.append(message) + + init_speed_tag = found_init.find(".//AbsoluteTargetSpeed") + if init_speed_tag is not None: + init_speed = init_speed_tag.attrib.get("value") + # Parse in declared parameter (remove the $) + if not is_float(init_speed): + init_speed = init_speed[1:] + else: + init_speed = 0 + + model = pedestrian.attrib.get("model") + + if self._invert_y: + world_pos_y = -float(world_pos_y) + + polygon_points = self.get_polygon_points( + world_pos_x, world_pos_y, world_pos_heading, "Pedestrian") + + walker_layer = QgsProject.instance().mapLayersByName("Pedestrians")[0] + entity_id = self.get_entity_id(walker_layer) + + feature = QgsFeature() + feature.setAttributes([entity_id, + model, + world_pos_heading, + world_pos_x, + world_pos_y, + world_pos_z, + init_speed]) + feature.setGeometry(QgsGeometry.fromPolygonXY([polygon_points])) + walker_layer.dataProvider().addFeature(feature) + + def parse_vehicle(self, vehicle, actor_name): + """ + Extracts information for vehicle and inserts into QGIS layer + + Args: + vehicle (XML element) + actor_name (string): actor name to match in Init and get positions + """ + # Query to get Init elements of same actor + query = f".//Private[@entityRef='{actor_name}']" + found_init = self._root.find(query) + + world_pos_x = 0 + world_pos_y = 0 + world_pos_z = 0 + world_pos_heading = 0 + world_pos = found_init.find(".//WorldPosition") + if world_pos is not None: + world_pos_x = world_pos.attrib.get("x") + world_pos_y = world_pos.attrib.get("y") + world_pos_z = world_pos.attrib.get("z") + world_pos_heading = world_pos.attrib.get("h") + else: + message = (f"Non WorldPosition waypoints are not supported (Entity: {actor_name})" + "Defaulting to WorldPos 0, 0, 0") + display_message(message, level="Info") + self._warning_message.append(message) + + init_speed_tag = found_init.find(".//AbsoluteTargetSpeed") + if init_speed_tag is not None: + init_speed = init_speed_tag.attrib.get("value") + # Parse in declared parameter (remove the $) + if not is_float(init_speed): + init_speed = init_speed[1:] + else: + init_speed = 0 + + vehicle_controller_tag = found_init.find(".//AssignControllerAction") + if vehicle_controller_tag is not None: + agent_tag = vehicle_controller_tag.find(".//Property") + agent = agent_tag.attrib.get("value") + if agent == "simple_vehicle_control": + agent_tag = vehicle_controller_tag.find(".//Property[@name='attach_camera']") + agent_camera = agent_tag.attrib.get("value") + agent_camera = True if agent_camera == "true" else False + else: + agent_camera = False + else: + agent = "simple_vehicle_control" + message = (f"No vehicle controller agent defined for {actor_name}, using " + "'simple_vehicle_control'") + self._warning_message.append(message) + display_message(message, level="Warning") + + model = vehicle.attrib.get("name") + + if self._invert_y: + world_pos_y = -float(world_pos_y) + + polygon_points = self.get_polygon_points( + world_pos_x, world_pos_y, world_pos_heading, "Vehicle") + + vehicle_layer = QgsProject.instance().mapLayersByName("Vehicles")[0] + entity_id = self.get_entity_id(vehicle_layer) + + feature = QgsFeature() + feature.setAttributes([ + entity_id, + model, + world_pos_heading, + world_pos_x, + world_pos_y, + world_pos_z, + init_speed, + agent, + agent_camera + ]) + feature.setGeometry(QgsGeometry.fromPolygonXY([polygon_points])) + vehicle_layer.dataProvider().addFeature(feature) + + def parse_prop(self, prop, actor_name): + """ + Extracts information for static objects and inserts into QGIS layer + + Args: + prop (XML element) + actor_name (string): actor name to match in Init and get positions + """ + # Query to get Init elements of same actor + query = f".//Private[@entityRef='{actor_name}']" + found_init = self._root.find(query) + + world_pos_x = 0 + world_pos_y = 0 + world_pos_z = 0 + world_pos_heading = 0 + world_pos = found_init.find(".//WorldPosition") + if world_pos is not None: + world_pos_x = world_pos.attrib.get("x") + world_pos_y = world_pos.attrib.get("y") + world_pos_z = world_pos.attrib.get("z") + world_pos_heading = world_pos.attrib.get("h") + else: + message = (f"Non WorldPosition waypoints are not supported (Entity: {actor_name})" + "Defaulting to WorldPos 0, 0, 0") + display_message(message, level="Info") + self._warning_message.append(message) + + model = prop.attrib.get("name") + model_type = prop.attrib.get("miscObjectCategory") + mass = prop.attrib.get("mass") + + if self._invert_y: + world_pos_y = -float(world_pos_y) + + physics = False + for prop_property in prop.iter("Property"): + physics = prop_property.attrib.get("value") + if physics == "on": + physics = True + else: + physics = False + + polygon_points = self.get_polygon_points( + world_pos_x, world_pos_y, world_pos_heading, "Prop") + + props_layer = QgsProject.instance().mapLayersByName("Static Objects")[0] + entity_id = self.get_entity_id(props_layer) + + feature = QgsFeature() + feature.setAttributes([ + entity_id, + model, + model_type, + world_pos_heading, + mass, + world_pos_x, + world_pos_y, + world_pos_z, + physics + ]) + feature.setGeometry(QgsGeometry.fromPolygonXY([polygon_points])) + props_layer.dataProvider().addFeature(feature) + + def get_entity_id(self, layer): + """ + Gets the largest entity ID from the layer. + If there are none, generates a new one. + + Args: + layer (QGIS layer): Layer to get entity ID from + + Returns: + [int]: Entity ID + """ + if layer.featureCount() != 0: + idx = layer.fields().indexFromName("id") + largest_id = layer.maximumValue(idx) + entity_id = largest_id + 1 + else: + entity_id = 1 + + return entity_id + + def get_polygon_points(self, pos_x, pos_y, angle, entity_type): + + angle = float(angle) + pos_x = float(pos_x) + pos_y = float(pos_y) + + if entity_type == "Pedestrian" or entity_type == "Vehicle": + if entity_type == "Pedestrian": + poly_edge_center = 0.4 + poly_edge_hor = 0.3 + poly_edge_ver = 0.35 + + if entity_type == "Vehicle": + poly_edge_center = 2.5 + poly_edge_hor = 2 + poly_edge_ver = 1 + + bot_left_x = pos_x + (-poly_edge_hor * math.cos(angle) - poly_edge_ver * math.sin(angle)) + bot_left_y = pos_y + (-poly_edge_hor * math.sin(angle) + poly_edge_ver * math.cos(angle)) + bot_right_x = pos_x + (-poly_edge_hor * math.cos(angle) + poly_edge_ver * math.sin(angle)) + bot_right_y = pos_y + (-poly_edge_hor * math.sin(angle) - poly_edge_ver * math.cos(angle)) + top_left_x = pos_x + (poly_edge_hor * math.cos(angle) - poly_edge_ver * math.sin(angle)) + top_left_y = pos_y + (poly_edge_hor * math.sin(angle) + poly_edge_ver * math.cos(angle)) + top_center_x = pos_x + poly_edge_center * math.cos(angle) + top_center_y = pos_y + poly_edge_center * math.sin(angle) + top_right_x = pos_x + (poly_edge_hor * math.cos(angle) + poly_edge_ver * math.sin(angle)) + top_right_y = pos_y + (poly_edge_hor * math.sin(angle) - poly_edge_ver * math.cos(angle)) + + # Create ENU points for polygon + bot_left = ad.map.point.createENUPoint(x=bot_left_x, y=bot_left_y, z=0) + bot_right = ad.map.point.createENUPoint(x=bot_right_x, y=bot_right_y, z=0) + top_left = ad.map.point.createENUPoint(x=top_left_x, y=top_left_y, z=0) + top_center = ad.map.point.createENUPoint(x=top_center_x, y=top_center_y, z=0) + top_right = ad.map.point.createENUPoint(x=top_right_x, y=top_right_y, z=0) + + # Convert back to Geo points + bot_left = ad.map.point.toGeo(bot_left) + bot_right = ad.map.point.toGeo(bot_right) + top_left = ad.map.point.toGeo(top_left) + top_center = ad.map.point.toGeo(top_center) + top_right = ad.map.point.toGeo(top_right) + + # Create polygon + polygon_points = [QgsPointXY(bot_left.longitude, bot_left.latitude), + QgsPointXY(bot_right.longitude, bot_right.latitude), + QgsPointXY(top_right.longitude, top_right.latitude), + QgsPointXY(top_center.longitude, top_center.latitude), + QgsPointXY(top_left.longitude, top_left.latitude)] + + return polygon_points + + elif entity_type == "Prop": + bot_left_x = pos_x + (-0.5 * math.cos(angle) - 0.5 * math.sin(angle)) + bot_left_y = pos_y + (-0.5 * math.sin(angle) + 0.5 * math.cos(angle)) + bot_right_x = pos_x + (-0.5 * math.cos(angle) + 0.5 * math.sin(angle)) + bot_right_y = pos_y + (-0.5 * math.sin(angle) - 0.5 * math.cos(angle)) + top_left_x = pos_x + (0.5 * math.cos(angle) - 0.5 * math.sin(angle)) + top_left_y = pos_y + (0.5 * math.sin(angle) + 0.5 * math.cos(angle)) + top_right_x = pos_x + (0.5 * math.cos(angle) + 0.5 * math.sin(angle)) + top_right_y = pos_y + (0.5 * math.sin(angle) - 0.5 * math.cos(angle)) + + # Create ENU points for polygon + bot_left = ad.map.point.createENUPoint(x=bot_left_x, y=bot_left_y, z=0) + bot_right = ad.map.point.createENUPoint(x=bot_right_x, y=bot_right_y, z=0) + top_left = ad.map.point.createENUPoint(x=top_left_x, y=top_left_y, z=0) + top_right = ad.map.point.createENUPoint(x=top_right_x, y=top_right_y, z=0) + + # Convert back to Geo points + bot_left = ad.map.point.toGeo(bot_left) + bot_right = ad.map.point.toGeo(bot_right) + top_left = ad.map.point.toGeo(top_left) + top_right = ad.map.point.toGeo(top_right) + + # Create polygon + polygon_points = [QgsPointXY(bot_left.longitude, bot_left.latitude), + QgsPointXY(bot_right.longitude, bot_right.latitude), + QgsPointXY(top_right.longitude, top_right.latitude), + QgsPointXY(top_left.longitude, top_left.latitude)] + + return polygon_points + + def parse_end_evals(self, end_eval_node): + """ + Parses end evaluation KPI information and saves into QGIS layers + + Args: + end_eval_node (XML element): Node that contains the end_evaluations + """ + # Clear existing paramters + end_eval_layer = QgsProject.instance().mapLayersByName("End Evaluation KPIs")[0] + current_features = [feat.id() for feat in end_eval_layer.getFeatures()] + end_eval_layer.dataProvider().deleteFeatures(current_features) + + for condition in end_eval_node.iter("Condition"): + cond_name = condition.attrib.get("name") + cond_edge = condition.attrib.get("conditionEdge") + delay = condition.attrib.get("delay") + + param_condition = condition.find(".//ParameterCondition") + param_ref = param_condition.attrib.get("parameterRef") + value = param_condition.attrib.get("value") + rule = param_condition.attrib.get("rule") + + feature = QgsFeature() + feature.setAttributes([ + cond_name, + float(delay), + cond_edge, + param_ref, + float(value), + rule + ]) + + end_eval_layer.dataProvider().addFeature(feature) + + def parse_maneuvers(self, story_node): + """ + Parses maneuver information and saves into QGIS layers + + Args: + story_node (XML element): Node that contains the maneuvers + """ + maneuver_layer = QgsProject.instance().mapLayersByName("Maneuvers")[0] + + for maneuver_group in story_node.iter("ManeuverGroup"): + # Default values (so attributes can be saved into QGIS) + # Will be changed based on what is parsed from OpenSCENARIO file + # Irrelevant information will be handled during export + man_id = self.get_entity_id(maneuver_layer) + man_type = "Entity Maneuvers" + entity = None + entity_act_type = "Waypoint" + global_act_type = "InfrastructureAction" + infra_traffic_id = 0 + infra_traffic_state = "green" + + entity_node = maneuver_group.find(".//Actors/EntityRef") + # For blank maneuvers / no maneuvers set + if entity_node is None: + message = ("Maneuver does not have an entity reference! " + "This maneuver will be skipped.") + display_message(message, level="Info") + self._warning_message.append(message) + break + entity = entity_node.attrib.get("entityRef") + + waypoint_act = maneuver_group.find(".//Maneuver/Event/Action/PrivateAction/RoutingAction") + if waypoint_act is None: + private_act_node = maneuver_group.find(".//Maneuver/Event/Action/PrivateAction") + private_act_type_node = list(private_act_node.iter())[1] + private_act_type = private_act_type_node.tag + if private_act_type == "LongitudinalAction": + entity_act_type = "Longitudinal" + self.parse_maneuvers_longitudinal(private_act_type, man_id) + elif private_act_type == "LateralAction": + entity_act_type = "Lateral" + self.parse_maneuvers_lateral(private_act_type, man_id) + else: + self.parse_waypoints(waypoint_act, man_id, entity) + + infra_act_node = maneuver_group.find(".//Maneuver/Event/Action/GlobalAction/InfrastructureAction") + if infra_act_node is None: + message = ("Infrastructure Action not found! " + "Import only supports infrastructure action currently.") + display_message(message, level="Info") + self._warning_message.append(message) + else: + man_type = "Global Actions" + traffic_signal_node = infra_act_node.find(".//TrafficSignalAction/TrafficSignalStateAction") + infra_traffic_id = traffic_signal_node.attrib.get("name")[3:] + infra_traffic_state = traffic_signal_node.attrib.get("state") + + # Start Triggers (Default Values) + start_trigger = "by Entity" + start_entity_cond = "EndOfRoadCondition" + start_entity_ref_entity = "" + start_entity_duration = 0 + start_entity_value = 0 + start_entity_rule = "lessThan" + start_entity_rel_dist_type = "cartesianDistance" + start_entity_frespace = False + start_entity_along_route = False + start_value_cond = "ParameterCondition" + start_value_param_ref = "" + start_value_name = "" + start_value_datetime = "2020-10-22T18:00:00" + start_value_value = 0 + start_value_rule = "lessThan" + start_value_state = "" + start_value_storyboard_type = "story" + start_value_storyboard_element = "" + start_value_storyboard_state = "completeState" + start_value_traffic_controller_ref = "" + start_value_traffic_controller_phase = "" + start_world_pos_tolerance = 0 + start_world_pos_x = 0 + start_world_pos_y = 0 + start_world_pos_z = 0 + start_world_pos_heading = 0 + + start_trigger_node = maneuver_group.find(".//Maneuver/Event/StartTrigger") + if start_trigger_node is not None: + # Check Entity Condition, if not, check Value Condition + condition_node = start_trigger_node.find(".//ConditionGroup/Condition/ByEntityCondition") + if condition_node is not None: + start_trigger = "by Entity" + entity_ref_node = condition_node.find(".//TriggeringEntities/EntityRef") + start_entity_ref_entity = entity_ref_node.attrib.get("entityRef") + + entity_cond_node = condition_node.find(".//EntityCondition") + entity_cond_node = list(entity_cond_node.iter())[1] + start_entity_cond = entity_cond_node.tag + + if "duration" in entity_cond_node.attrib: + start_entity_duration = entity_cond_node.attrib.get("duration") + + if "entityRef" in entity_cond_node.attrib: + start_entity_ref_entity = entity_cond_node.attrib.get("entityRef") + + if "value" in entity_cond_node.attrib: + start_entity_value = entity_cond_node.attrib.get("value") + + if "freespace" in entity_cond_node.attrib: + if entity_cond_node.attrib.get("freespace") == "true": + start_entity_frespace = True + else: + start_entity_frespace = False + + if "alongRoute" in entity_cond_node.attrib: + if entity_cond_node.attrib.get("alongRoute") == "true": + start_entity_along_route = True + else: + start_entity_along_route = False + + if "rule" in entity_cond_node.attrib: + start_entity_rule = entity_cond_node.attrib.get("rule") + + if "tolerance" in entity_cond_node.attrib: + start_world_pos_tolerance = entity_cond_node.attrib.get("tolerance") + world_pos_node = entity_cond_node.find(".//Position/WorldPosition") + + if world_pos_node is not None: + start_world_pos_x = float(world_pos_node.attrib.get("x")) + start_world_pos_y = float(world_pos_node.attrib.get("y")) + start_world_pos_z = float(world_pos_node.attrib.get("z")) + start_world_pos_heading = float(world_pos_node.attrib.get("h")) + else: + message = ("Non WorldPosition waypoints are not supported (Maneuver ID: " + f"{str(man_id)} Entity: {entity}) Defaulting to WorldPos 0, 0, 0") + display_message(message, level="Info") + self._warning_message.append(message) + + else: + condition_node = start_trigger_node.find(".//ConditionGroup/Condition/ByValueCondition") + start_trigger = "by Value" + value_cond_node = list(condition_node.iter())[1] + start_value_cond = value_cond_node.tag + + if "parameterRef" in value_cond_node.attrib: + start_value_param_ref = value_cond_node.attrib.get("parameterRef") + + if "name" in value_cond_node.attrib: + start_value_name = value_cond_node.attrib.get("name") + + if "value" in value_cond_node.attrib: + start_value_value = value_cond_node.attrib.get("value") + + if "rule" in value_cond_node.attrib: + start_value_rule = value_cond_node.attrib.get("rule") + + if "state" in value_cond_node.attrib: + start_value_state = value_cond_node.attrib.get("state") + + if "storyboardElementType" in value_cond_node.attrib: + start_value_storyboard_type = value_cond_node.attrib.get("storyboardElementType") + start_value_storyboard_element = value_cond_node.attrib.get("storyboardElementRef") + start_value_storyboard_state = value_cond_node.attrib.get("state") + + if "trafficSignalControllerRef" in value_cond_node.attrib: + start_value_traffic_controller_ref = value_cond_node.attrib.get("trafficSignalControllerRef") + start_value_traffic_controller_phase = value_cond_node.attrib.get("phase") + + # Stop Triggers (Default Values) + stop_trigger_enabled = False + stop_trigger = "by Entity" + stop_entity_cond = "EndOfRoadCondition" + stop_entity_ref_entity = "" + stop_entity_duration = 0 + stop_entity_value = 0 + stop_entity_rule = "lessThan" + stop_entity_rel_dist_type = "cartesianDistance" + stop_entity_freespace = False + stop_entity_along_route = False + stop_value_cond = "ParameterCondition" + stop_value_param_ref = "" + stop_value_name = "" + stop_value_datetime = "2020-10-22T18:00:00" + stop_value_value = 0 + stop_value_rule = "lessThan" + stop_value_state = "" + stop_value_storyboard_type = "story" + stop_value_storyboard_element = "" + stop_value_storyboard_state = "completeState" + stop_value_traffic_controller_ref = "" + stop_value_traffic_controller_phase = "" + stop_world_pos_tolerance = 0 + stop_world_pos_x = 0 + stop_world_pos_y = 0 + stop_world_pos_z = 0 + stop_world_pos_heading = 0 + + stop_trigger_node = maneuver_group.find(".//Maneuver/Event/StopTrigger") + if stop_trigger_node is not None: + stop_trigger_enabled = True + # Check Entity Condition, if not, check Value Condition + condition_node = stop_trigger_node.find(".//ConditionGroup/Condition/ByEntityCondition") + if condition_node is not None: + stop_trigger = "by Entity" + entity_ref_node = condition_node.find(".//TriggeringEntities/EntityRef") + stop_entity_ref_entity = entity_ref_node.attrib.get("entityRef") + + entity_cond_node = condition_node.find(".//EntityCondition") + entity_cond_node = list(entity_cond_node.iter())[1] + stop_entity_cond = entity_cond_node.tag + + if "duration" in entity_cond_node.attrib: + stop_entity_duration = entity_cond_node.attrib.get("duration") + + if "entityRef" in entity_cond_node.attrib: + stop_entity_ref_entity = entity_cond_node.attrib.get("entityRef") + + if "value" in entity_cond_node.attrib: + stop_entity_value = entity_cond_node.attrib.get("value") + + if "freespace" in entity_cond_node.attrib: + if entity_cond_node.attrib.get("freespace") == "true": + stop_entity_frespace = True + else: + stop_entity_frespace = False + + if "alongRoute" in entity_cond_node.attrib: + if entity_cond_node.attrib.get("alongRoute") == "true": + stop_entity_along_route = True + else: + stop_entity_along_route = False + + if "rule" in entity_cond_node.attrib: + stop_entity_rule = entity_cond_node.attrib.get("rule") + + if "tolerance" in entity_cond_node.attrib: + stop_world_pos_tolerance = entity_cond_node.attrib.get("tolerance") + world_pos_node = entity_cond_node.find(".//Position/WorldPosition") + + if world_pos_node is not None: + stop_world_pos_x = float(world_pos_node.attrib.get("x")) + stop_world_pos_y = float(world_pos_node.attrib.get("y")) + stop_world_pos_z = float(world_pos_node.attrib.get("z")) + stop_world_pos_heading = float(world_pos_node.attrib.get("h")) + else: + message = ("Non WorldPosition waypoints are not supported (Maneuver ID: " + f"{str(man_id)} Entity: {entity}) Defaulting to WorldPos 0, 0, 0") + display_message(message, level="Info") + self._warning_message.append(message) + + else: + condition_node = stop_trigger_node.find(".//ConditionGroup/Condition/ByValueCondition") + stop_trigger = "by Value" + value_cond_node = list(condition_node.iter())[1] + stop_value_cond = value_cond_node.tag + + if "parameterRef" in value_cond_node.attrib: + stop_value_param_ref = value_cond_node.attrib.get("parameterRef") + + if "name" in value_cond_node.attrib: + stop_value_name = value_cond_node.attrib.get("name") + + if "value" in value_cond_node.attrib: + stop_value_value = value_cond_node.attrib.get("value") + + if "rule" in value_cond_node.attrib: + stop_value_rule = value_cond_node.attrib.get("rule") + + if "state" in value_cond_node.attrib: + stop_value_state = value_cond_node.attrib.get("state") + + if "storyboardElementType" in value_cond_node.attrib: + stop_value_storyboard_type = value_cond_node.attrib.get("storyboardElementType") + stop_value_storyboard_element = value_cond_node.attrib.get("storyboardElementRef") + stop_value_storyboard_state = value_cond_node.attrib.get("state") + + if "trafficSignalControllerRef" in value_cond_node.attrib: + stop_value_traffic_controller_ref = value_cond_node.attrib.get("trafficSignalControllerRef") + stop_value_traffic_controller_phase = value_cond_node.attrib.get("phase") + + feature = QgsFeature() + feature.setAttributes([ + man_id, + man_type, + entity, + entity_act_type, + global_act_type, + infra_traffic_id, + infra_traffic_state, + start_trigger, + start_entity_cond, + start_entity_ref_entity, + start_entity_duration, + start_entity_value, + start_entity_rule, + start_entity_rel_dist_type, + start_entity_frespace, + start_entity_along_route, + start_value_cond, + start_value_param_ref, + start_value_name, + start_value_datetime, + start_value_value, + start_value_rule, + start_value_state, + start_value_storyboard_type, + start_value_storyboard_element, + start_value_storyboard_state, + start_value_traffic_controller_ref, + start_value_traffic_controller_phase, + start_world_pos_tolerance, + start_world_pos_x, + start_world_pos_y, + start_world_pos_z, + start_world_pos_heading, + stop_trigger_enabled, + stop_trigger, + stop_entity_cond, + stop_entity_ref_entity, + stop_entity_duration, + stop_entity_value, + stop_entity_rule, + stop_entity_rel_dist_type, + stop_entity_freespace, + stop_entity_along_route, + stop_value_cond, + stop_value_param_ref, + stop_value_name, + stop_value_datetime, + stop_value_value, + stop_value_rule, + stop_value_state, + stop_value_storyboard_type, + stop_value_storyboard_element, + stop_value_storyboard_state, + stop_value_traffic_controller_ref, + stop_value_traffic_controller_phase, + stop_world_pos_tolerance, + stop_world_pos_x, + stop_world_pos_y, + stop_world_pos_z, + stop_world_pos_heading + ]) + maneuver_layer.dataProvider().addFeature(feature) + + def parse_waypoints(self, waypoint_node, man_id, entity): + """ + Parses waypoint maneuvers and saves into QGIS layers + + Args: + waypoint_node (XML element): Node that contains RoutingAction + man_id (int): Maneuver ID to differentiate maneuvers + entity (str): Entity name for maneuver + """ + waypoint_layer = QgsProject.instance().mapLayersByName("Waypoint Maneuvers")[0] + + world_pos_x = 0 + world_pos_y = 0 + world_pos_z = 0 + world_pos_heading = 0 + waypoint_id = 1 + + for waypoint in waypoint_node.iter("Waypoint"): + route_strat = waypoint.attrib.get("routeStrategy") + world_pos_node = waypoint.find(".//Position/WorldPosition") + + if world_pos_node is not None: + world_pos_x = float(world_pos_node.attrib.get("x")) + world_pos_y = float(world_pos_node.attrib.get("y")) + world_pos_z = float(world_pos_node.attrib.get("z")) + world_pos_heading = float(world_pos_node.attrib.get("h")) + else: + message = ("Non WorldPosition waypoints are not supported (Maneuver ID: " + f"{str(man_id)} Entity: {entity})") + display_message(message, level="Info") + self._warning_message.append(message) + break + + feature = QgsFeature() + feature.setAttributes([ + man_id, + entity, + waypoint_id, + world_pos_heading, + world_pos_x, + world_pos_y, + world_pos_z, + route_strat + ]) + + # Create ENU point and convert to GEO for display in QGIS + enupoint = ad.map.point.createENUPoint(world_pos_x, world_pos_y, world_pos_z) + geopoint = ad.map.point.toGeo(enupoint) + feature.setGeometry( + QgsGeometry.fromPointXY(QgsPointXY(geopoint.longitude, geopoint.latitude))) + waypoint_layer.dataProvider().addFeature(feature) + + waypoint_id += 1 + + def parse_maneuvers_longitudinal(self, long_act_node, man_id): + """ + Parse longitudinal maneuvers and saves into QGIS layers + + Args: + long_act_node (XML element): XML node that contains LongiduinalAction + man_id (int): Maneuver ID to differentiate maneuvers + """ + long_man_layer = QgsProject.instance().mapLayersByName("Longitudinal Maneuvers")[0] + + # Default values + long_type = "SpeedAction" + speed_target = "RelativeTargetSpeed" + entity_ref = "" + dynamics_shape = "linear" + dynamics_dimension = "rate" + dynamics_value = "0" + target_type = "delta" + target_speed = "0" + continuous = True + freespace = True + max_accel = "0" + max_decel = "0" + max_speed = "0" + + long_type_node = list(long_act_node.iter())[1] + long_type = long_type_node.tag + if long_type == "SpeedAction": + speed_dynamics_node = long_type_node.find(".//SpeedAction/SpeedActionDynamics") + dynamics_shape = speed_dynamics_node.attrib.get("dynamicsShape") + dynamics_value = speed_dynamics_node.attrib.get("value") + dynamics_dimension = speed_dynamics_node.attrib.get("dynamicsDimension") + + speed_target_node = long_act_node.find(".//SpeedAction/SpeedActionTarget") + speed_target_node = list(speed_target_node.iter())[1] + speed_target = speed_target_node.tag + + if speed_target == "RelativeTargetSpeed": + rel_target_speed_node = speed_target_node.find(".//RelativeTargetSpeed") + entity_ref = rel_target_speed_node.attrib.get("entityRef") + target_speed = rel_target_speed_node.attrib.get("value") + target_type = rel_target_speed_node.attrib.get("speedTargetValueType") + if rel_target_speed_node.attrib.get("continuous") == "true": + continuous = True + else: + continuous = False + elif speed_target == "AbsoluteTargetSpeed": + abs_target_speed_node = speed_target_node.find(".//AbsoluteTargetSpeed") + target_speed = abs_target_speed_node.attrib.get("value") + + elif long_type == "LongitudinalDistanceAction": + entity_ref = long_act_node.attrib.get("entityRef") + if long_act_node.attrib.get("freespace") == "true": + freespace = True + else: + freespace = False + if long_act_node.attrib.get("continuous") == "true": + continuous = True + else: + continuous = False + + dynamic_constrain_node = long_act_node.find(".//LongitudinalDistanceAction/DynamicConstraints") + max_accel = dynamic_constrain_node.attrib.get("maxAcceleration") + max_decel = dynamic_constrain_node.attrib.get("maxDeceleration") + max_speed = dynamic_constrain_node.attrib.get("maxSpeed") + + feature = QgsFeature() + feature.setAttributes([ + man_id, + long_type, + speed_target, + entity_ref, + dynamics_shape, + dynamics_dimension, + dynamics_value, + target_type, + target_speed, + continuous, + freespace, + max_accel, + max_decel, + max_speed + ]) + long_man_layer.dataProvider().addFeature(feature) + + def parse_maneuvers_lateral(self, lat_act_node, man_id): + """ + Parse longitudinal maneuvers and saves into QGIS layers + + Args: + lat_act_node (XML element): XML node that contains LateralAction + man_id (int): Maneuver ID to differentiate maneuvers + """ + lat_man_layer = QgsProject.instance().mapLayersByName("Lateral Maneuvers")[0] + + # Default values + lat_type = "LaneChangeAction" + lane_target = "RelativeTargetLane" + entity_ref = "" + dynamics_shape = "linear" + dynamics_dimension = "rate" + dynamics_value = "0" + lane_target_value = "0" + max_lat_accel = "0" + max_accel = "0" + max_decel = "0" + max_speed = "0" + + lat_type_node = list(lat_act_node.iter())[1] + lat_type = lat_type_node.tag + + if lat_type == "LaneChangeAction": + dynamics_node = lat_type_node.find(".//LaneChangeAction/LaneChangeActionDynamics") + dynamics_shape = dynamics_node.attrib.get("dynamicsShape") + dynamics_value = dynamics_node.attrib.get("value") + dynamics_dimension = dynamics_node.attrib.get("dynamicsDimension") + + lane_target_node = lat_type_node.find(".//LaneChangeAction/LaneChangeTarget") + lane_target_choice_node = list(lane_target_node.iter())[1] + lane_target = lane_target_choice_node.tag + if lane_target == "RelativeTargetLane": + rel_target_node = lane_target_choice_node.find(".//RelativeTargetLane") + entity_ref = rel_target_node.attrib.get("entityRef") + lane_target_value = rel_target_node.attrib.get("value") + elif lane_target == "AbsoluteTargetLane": + abs_target_node = lane_target_choice_node.find(".//AbsoluteTargetLane") + lane_target_value = abs_target_node.attrib.get("value") + + elif lat_type == "LaneOffsetAction": + dynamics_node = lat_type_node.find(".//LaneOffsetAction/LaneOffsetActionDynamics") + max_lat_accel = dynamics_node.attrib.get("maxLateralAcc") + dynamics_shape = dynamics_node.attrib.get("dynamicsShape") + + lane_target_node = lat_act_node.find(".//LaneOffsetAction/LaneOffsetTarget") + lane_target_choice_node = list(lane_target_node.iter())[1] + lane_target = lane_target_choice_node.tag + if lane_target == "RelativeTargetLaneOffset": + rel_target_node = lane_target_choice_node.find(".//RelativeTargetLaneOffset") + entity_ref = rel_target_node.attrib.get("entityRef") + lane_target_value = rel_target_node.attrib.get("value") + elif lane_target == "AbsoluteTargetLaneOffset": + abs_target_node = lane_target_choice_node.find(".//AbsoluteTargetLaneOffset") + lane_target_value = abs_target_node.attrib.get("value") + + elif lat_type == "LateralDistanceAction": + dynamic_constrain_node = lat_type_node.find(".//LateralDistanceAction/DynamicConstraints") + max_accel = dynamic_constrain_node.attrib.get("maxAcceleration") + max_decel = dynamic_constrain_node.attrib.get("maxDeceleration") + max_speed = dynamic_constrain_node.attrib.get("maxSpeed") + + feature = QgsFeature() + feature.setAttributes([ + man_id, + lat_type, + lane_target, + entity_ref, + dynamics_shape, + dynamics_dimension, + dynamics_value, + lane_target_value, + max_lat_accel, + max_accel, + max_decel, + max_speed + ]) + lat_man_layer.dataProvider().addFeature(feature) diff --git a/osc_generator/import_xosc_dialog.ui b/osc_generator/import_xosc_dialog.ui new file mode 100644 index 0000000..72c267b --- /dev/null +++ b/osc_generator/import_xosc_dialog.ui @@ -0,0 +1,84 @@ + + + Dialog + + + + 0 + 0 + 380 + 150 + + + + Import XOSC + + + + + + Import File + + + + + + + ... + + + + + + + + + + Qt::Horizontal + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok + + + true + + + + + + + + + button_box + accepted() + Dialog + accept() + + + 278 + 190 + + + 157 + 169 + + + + + button_box + rejected() + Dialog + reject() + + + 278 + 190 + + + 286 + 169 + + + + + diff --git a/osc_generator/parameter_declarations.py b/osc_generator/parameter_declarations.py index 6b4844f..fafba38 100644 --- a/osc_generator/parameter_declarations.py +++ b/osc_generator/parameter_declarations.py @@ -8,14 +8,14 @@ OpenSCENARIO Generator - Parameter Declarations """ import os +from osc_generator.helper_functions import HelperFunctions # pylint: disable=no-name-in-module, no-member -from qgis.core import (Qgis, QgsFeature, QgsField, QgsMessageLog, QgsProject, - QgsVectorLayer, QgsExpression, QgsFeatureRequest) -from qgis.gui import QgsMapTool +from qgis.core import Qgis, QgsFeature, QgsProject, QgsFeatureRequest from qgis.PyQt import QtWidgets, uic -from qgis.PyQt.QtCore import Qt, QVariant, pyqtSignal +from qgis.PyQt.QtCore import Qt, pyqtSignal from qgis.utils import iface from PyQt5.QtWidgets import QMessageBox +from .helper_functions import layer_setup_parameters, display_message FORM_CLASS, _ = uic.loadUiType(os.path.join( os.path.dirname(__file__), 'parameter_declarations.ui')) @@ -35,11 +35,11 @@ def __init__(self, parent=None): self.setupUi(self) self.add_param_button.pressed.connect(self.add_parameters) - self._param_layer = None - self._param_layer_data_input = None self._canvas = iface.mapCanvas() - self.layer_setup() + layer_setup_parameters() + self._param_layer = QgsProject.instance().mapLayersByName("Parameter Declarations")[0] + self._param_layer_data_input = self._param_layer.dataProvider() def closeEvent(self, event): """ @@ -48,31 +48,6 @@ def closeEvent(self, event): self.closingPlugin.emit() event.accept() - def layer_setup(self): - """ - Sets up layer for storing parameter declarations - """ - root_layer = QgsProject.instance().layerTreeRoot() - osc_layer = root_layer.findGroup("OpenSCENARIO") - if not QgsProject.instance().mapLayersByName("Parameter Declarations"): - param_layer = QgsVectorLayer("None", "Parameter Declarations", "memory") - QgsProject.instance().addMapLayer(param_layer, False) - osc_layer.addLayer(param_layer) - # Setup layer attributes - data_attributes = [QgsField("Parameter Name", QVariant.String), - QgsField("Type", QVariant.String), - QgsField("Value", QVariant.String)] - data_input = param_layer.dataProvider() - data_input.addAttributes(data_attributes) - param_layer.updateFields() - - message = "Parameter declarations layer added" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) - - self._param_layer = QgsProject.instance().mapLayersByName("Parameter Declarations")[0] - self._param_layer_data_input = self._param_layer.dataProvider() - def add_parameters(self): """ Adds parameters into layer @@ -87,8 +62,7 @@ def add_parameters(self): self.insert_parameters(param_name, param_type, param_value) else: message = f"Parameter '{param_name}' exists!" - iface.messageBar().pushMessage("Info", message, level=Qgis.Warning) - QgsMessageLog.logMessage(message, level=Qgis.Warning) + display_message(message, level="Warning") query = f'"Parameter Name" = \'{param_name}\'' request = QgsFeatureRequest().setFilterExpression(query) @@ -138,9 +112,9 @@ def insert_parameters(self, param_name, param_type, param_value): Inserts parameters into attributes table Args: - param_name ([string]): Parameter name - param_type ([string]): Parameter type - param_value ([string]): Parameter value + param_name (string): Parameter name + param_type (string): Parameter type + param_value (string): Parameter value """ feature = QgsFeature() feature.setAttributes([param_name, @@ -149,10 +123,15 @@ def insert_parameters(self, param_name, param_type, param_value): self._param_layer_data_input.addFeature(feature) message = f"Parameter '{param_name}' added!" - iface.messageBar().pushMessage("Info", message, level=Qgis.Info) - QgsMessageLog.logMessage(message, level=Qgis.Info) + display_message(message, level="Info") def delete_existing_parameter(self, param_name): + """ + Delete existing parameter from attributes table + + Args: + param_name (string): Parameter name to be deleted + """ query = f'"Parameter Name" = \'{param_name}\'' feature_request = QgsFeatureRequest().setFilterExpression(query) features_to_delete = self._param_layer.getFeatures(feature_request)