From 65cc18914e58906c1fafe2336a45119c202152c6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 22 Nov 2021 12:36:07 -0800 Subject: [PATCH 1/2] Added lidar component inspector (#1203) * Support editing air pressure sensor in the GUI Signed-off-by: Nate Koenig * Add noise to qrc Signed-off-by: Nate Koenig * Add noise to qrc Signed-off-by: Nate Koenig * Added altimeter sensor inspector Signed-off-by: Nate Koenig * Added magnetometer inspector Signed-off-by: Nate Koenig * Fix lint Signed-off-by: Michael Carroll * Update sensor icon Signed-off-by: Nate Koenig * Move AirPressure functions out of ComponentInspector (#1179) Signed-off-by: Louise Poubel * Fix get decimals, and address comments Signed-off-by: Nate Koenig * cleanup and simplification Signed-off-by: Nate Koenig * Require sdf 12.1.0 Signed-off-by: Nate Koenig * missign width Signed-off-by: Nate Koenig * Added simulation state aware spin box Signed-off-by: Nate Koenig * Merged Signed-off-by: Nate Koenig * merged Signed-off-by: Nate Koenig * Remove console output Signed-off-by: Nate Koenig * alphabetize Signed-off-by: Nate Koenig * Fix build Signed-off-by: Nate Koenig * Add IMU component inspector Signed-off-by: Nate Koenig * Added lidar component inspector Signed-off-by: Nate Koenig * Fix codecheck Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Michael Carroll Co-authored-by: Louise Poubel --- examples/worlds/sensors.sdf | 24 + src/cmd/ModelCommandAPI.cc | 48 ++ .../component_inspector/CMakeLists.txt | 2 + .../component_inspector/ComponentInspector.cc | 7 + .../ComponentInspector.qrc | 2 + src/gui/plugins/component_inspector/Lidar.cc | 153 ++++++ src/gui/plugins/component_inspector/Lidar.hh | 66 +++ src/gui/plugins/component_inspector/Lidar.qml | 451 ++++++++++++++++++ .../plugins/component_inspector/LidarScan.qml | 245 ++++++++++ src/gui/plugins/component_inspector/Noise.qml | 12 +- .../component_inspector/StateAwareSpin.qml | 2 +- 11 files changed, 1005 insertions(+), 7 deletions(-) create mode 100644 src/gui/plugins/component_inspector/Lidar.cc create mode 100644 src/gui/plugins/component_inspector/Lidar.hh create mode 100644 src/gui/plugins/component_inspector/Lidar.qml create mode 100644 src/gui/plugins/component_inspector/LidarScan.qml diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index eefd6a8ee8..8574650335 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -183,6 +183,30 @@ + + 0.43 0 0.26 0 0 0 + 20 + + + + 720 + 1 + -2.3562 + 2.3562 + + + + 0.04 + 5 + 0.01 + + + gaussian + 0 + 0.01 + + + diff --git a/src/cmd/ModelCommandAPI.cc b/src/cmd/ModelCommandAPI.cc index 705a59bde8..773ac405ec 100644 --- a/src/cmd/ModelCommandAPI.cc +++ b/src/cmd/ModelCommandAPI.cc @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -39,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -459,6 +461,51 @@ void printImu(const uint64_t _entity, const EntityComponentManager &_ecm, << "- Orientation enabled:" << imu->OrientationEnabled() << std::endl; } +////////////////////////////////////////////////// +void printGpuLidar(const uint64_t _entity, + const EntityComponentManager &_ecm, int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Lidar *lidar = sensor.LidarSensor(); + + std::cout << std::string(_spaces, ' ') << "- Range:\n"; + std::cout << std::string(_spaces+2, ' ') << "- Min: " + << lidar->RangeMin() << std::endl; + std::cout << std::string(_spaces+2, ' ') << "- Max: " + << lidar->RangeMax() << std::endl; + std::cout << std::string(_spaces+2, ' ') << "- Resolution: " + << lidar->RangeResolution() << std::endl; + + std::cout << std::string(_spaces, ' ') << "- Horizontal scan:\n"; + std::cout << std::string(_spaces+2, ' ') << "- Samples: " + << lidar->HorizontalScanSamples() << std::endl; + std::cout << std::string(_spaces+2, ' ') << "- Resolution: " + << lidar->HorizontalScanResolution() << std::endl; + std::cout << std::string(_spaces+2, ' ') << "- Min angle: " + << lidar->HorizontalScanMinAngle() << std::endl; + std::cout << std::string(_spaces+2, ' ') << "- Max angle: " + << lidar->HorizontalScanMaxAngle() << std::endl; + + std::cout << std::string(_spaces, ' ') << "- Vertical scan:\n"; + std::cout << std::string(_spaces+2, ' ') << "- Samples: " + << lidar->VerticalScanSamples() << std::endl; + std::cout << std::string(_spaces+2, ' ') << "- Resolution: " + << lidar->VerticalScanResolution() << std::endl; + std::cout << std::string(_spaces+2, ' ') << "- Min angle: " + << lidar->VerticalScanMinAngle() << std::endl; + std::cout << std::string(_spaces+2, ' ') << "- Max angle: " + << lidar->VerticalScanMaxAngle() << std::endl; + + std::cout << std::string(_spaces, ' ') << "- Noise:\n"; + printNoise(lidar->LidarNoise(), _spaces + 2); +} + ////////////////////////////////////////////////// void printMagnetometer(const uint64_t _entity, const EntityComponentManager &_ecm, int _spaces) @@ -633,6 +680,7 @@ void printLinks(const uint64_t _modelEntity, printAirPressure(sensor, _ecm, spaces + 2); printAltimeter(sensor, _ecm, spaces + 2); printCamera(sensor, _ecm, spaces + 2); + printGpuLidar(sensor, _ecm, spaces + 2); printImu(sensor, _ecm, spaces + 2); printMagnetometer(sensor, _ecm, spaces + 2); printRgbdCamera(sensor, _ecm, spaces + 2); diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt index 8a0f463e63..4c653e6a60 100644 --- a/src/gui/plugins/component_inspector/CMakeLists.txt +++ b/src/gui/plugins/component_inspector/CMakeLists.txt @@ -4,6 +4,7 @@ gz_add_gui_plugin(ComponentInspector Altimeter.cc ComponentInspector.cc Imu.cc + Lidar.cc Magnetometer.cc ModelEditor.cc Types.cc @@ -12,6 +13,7 @@ gz_add_gui_plugin(ComponentInspector Altimeter.hh ComponentInspector.hh Imu.hh + Lidar.hh Magnetometer.hh ModelEditor.hh Types.hh diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 07c068243d..eaf2468e93 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -74,6 +74,7 @@ #include "Altimeter.hh" #include "ComponentInspector.hh" #include "Imu.hh" +#include "Lidar.hh" #include "Magnetometer.hh" #include "ModelEditor.hh" @@ -126,6 +127,9 @@ namespace ignition::gazebo /// \brief Imu inspector elements public: std::unique_ptr imu; + /// \brief Lidar inspector elements + public: std::unique_ptr lidar; + /// \brief Magnetometer inspector elements public: std::unique_ptr magnetometer; @@ -451,6 +455,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *) // Create the imu this->dataPtr->imu = std::make_unique(this); + // Create the lidar + this->dataPtr->lidar = std::make_unique(this); + // Create the magnetometer this->dataPtr->magnetometer = std::make_unique(this); } diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index c802a22a43..9508c226fa 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -6,6 +6,8 @@ ComponentInspector.qml ExpandingTypeHeader.qml Imu.qml + Lidar.qml + LidarScan.qml Light.qml Magnetometer.qml NoData.qml diff --git a/src/gui/plugins/component_inspector/Lidar.cc b/src/gui/plugins/component_inspector/Lidar.cc new file mode 100644 index 0000000000..48ad47b2fc --- /dev/null +++ b/src/gui/plugins/component_inspector/Lidar.cc @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include + +#include +#include +#include + +#include "ComponentInspector.hh" +#include "Lidar.hh" +#include "Types.hh" + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +Lidar::Lidar(ComponentInspector *_inspector) +{ + _inspector->Context()->setContextProperty("LidarImpl", this); + this->inspector = _inspector; + + ComponentCreator creator = + [=](EntityComponentManager &_ecm, Entity _entity, QStandardItem *_item) + { + auto comp = _ecm.Component(_entity); + if (nullptr == _item || nullptr == comp) + return; + const sdf::Lidar *lidar = comp->Data().LidarSensor(); + + _item->setData(QString("Lidar"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(lidar->LidarNoise().Mean()), + QVariant(lidar->LidarNoise().BiasMean()), + QVariant(lidar->LidarNoise().StdDev()), + QVariant(lidar->LidarNoise().BiasStdDev()), + QVariant(lidar->LidarNoise().DynamicBiasStdDev()), + QVariant(lidar->LidarNoise().DynamicBiasCorrelationTime()), + + QVariant(lidar->HorizontalScanSamples()), + QVariant(lidar->HorizontalScanResolution()), + QVariant(lidar->HorizontalScanMinAngle().Radian()), + QVariant(lidar->HorizontalScanMaxAngle().Radian()), + + QVariant(lidar->VerticalScanSamples()), + QVariant(lidar->VerticalScanResolution()), + QVariant(lidar->VerticalScanMinAngle().Radian()), + QVariant(lidar->VerticalScanMaxAngle().Radian()), + + QVariant(lidar->RangeMin()), + QVariant(lidar->RangeMax()), + QVariant(lidar->RangeResolution()), + + }), ComponentsModel::RoleNames().key("data")); + }; + + this->inspector->RegisterComponentCreator( + components::GpuLidar::typeId, creator); +} + +///////////////////////////////////////////////// +Q_INVOKABLE void Lidar::OnLidarNoise( + double _mean, double _meanBias, double _stdDev, + double _stdDevBias, double _dynamicBiasStdDev, + double _dynamicBiasCorrelationTime) +{ + ignition::gazebo::UpdateCallback cb = + [=](EntityComponentManager &_ecm) + { + auto comp = _ecm.Component( + this->inspector->Entity()); + if (comp) + { + sdf::Lidar *lidar = comp->Data().LidarSensor(); + if (lidar) + { + sdf::Noise noise = lidar->LidarNoise(); + + setNoise(noise, _mean, _meanBias, _stdDev, _stdDevBias, + _dynamicBiasStdDev, _dynamicBiasCorrelationTime); + + lidar->SetLidarNoise(noise); + } + else + ignerr << "Unable to get the lidar noise data.\n"; + } + else + { + ignerr << "Unable to get the lidar component.\n"; + } + }; + this->inspector->AddUpdateCallback(cb); +} + +///////////////////////////////////////////////// +Q_INVOKABLE void Lidar::OnLidarChange( + double _rangeMin, double _rangeMax, + double _rangeResolution, double _horizontalScanSamples, + double _horizontalScanResolution, + double _horizontalScanMinAngle, + double _horizontalScanMaxAngle, double _verticalScanSamples, + double _verticalScanResolution, double _verticalScanMinAngle, + double _verticalScanMaxAngle) +{ + ignition::gazebo::UpdateCallback cb = + [=](EntityComponentManager &_ecm) + { + auto comp = _ecm.Component( + this->inspector->Entity()); + if (comp) + { + sdf::Lidar *lidar = comp->Data().LidarSensor(); + if (lidar) + { + lidar->SetRangeMin(_rangeMin); + lidar->SetRangeMax(_rangeMax); + lidar->SetRangeResolution(_rangeResolution); + + lidar->SetHorizontalScanSamples(_horizontalScanSamples); + lidar->SetHorizontalScanResolution(_horizontalScanResolution); + lidar->SetHorizontalScanMinAngle(_horizontalScanMinAngle); + lidar->SetHorizontalScanMaxAngle(_horizontalScanMaxAngle); + + lidar->SetVerticalScanSamples(_verticalScanSamples); + lidar->SetVerticalScanResolution(_verticalScanResolution); + lidar->SetVerticalScanMinAngle(_verticalScanMinAngle); + lidar->SetVerticalScanMaxAngle(_verticalScanMaxAngle); + } + else + ignerr << "Unable to get the lidar data.\n"; + } + else + { + ignerr << "Unable to get the lidar component.\n"; + } + }; + this->inspector->AddUpdateCallback(cb); +} + diff --git a/src/gui/plugins/component_inspector/Lidar.hh b/src/gui/plugins/component_inspector/Lidar.hh new file mode 100644 index 0000000000..a6bc280751 --- /dev/null +++ b/src/gui/plugins/component_inspector/Lidar.hh @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_LIDAR_HH_ +#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_LIDAR_HH_ + +#include + +namespace ignition +{ +namespace gazebo +{ + class ComponentInspector; + + /// \brief A class that handles Lidar sensor changes. + class Lidar : public QObject + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _inspector The component inspector. + public: explicit Lidar(ComponentInspector *_inspector); + + /// \brief This function is called when a user changes values in the + /// Lidar sensor's linear acceleration X noise values. + /// \param[in] _mean Mean value + /// \param[in] _meanBias Bias mean value + /// \param[in] _stdDev Standard deviation value + /// \param[in] _stdDevBias Bias standard deviation value + /// \param[in] _dynamicBiasStdDev Dynamic bias standard deviation value + /// \param[in] _dynamicBiasCorrelationTime Dynamic bias correlation time + /// value + public: Q_INVOKABLE void OnLidarNoise( + double _mean, double _meanBias, double _stdDev, + double _stdDevBias, double _dynamicBiasStdDev, + double _dynamicBiasCorrelationTime); + + public: Q_INVOKABLE void OnLidarChange( + double _rangeMin, double _rangeMax, + double _rangeResolution, double _horizontalScanSamples, + double _horizontalScanResolution, + double _horizontalScanMinAngle, + double _horizontalScanMaxAngle, double _verticalScanSamples, + double _verticalScanResolution, double _verticalScanMinAngle, + double _verticalScanMaxAngle); + + /// \brief Pointer to the component inspector. This is used to add + /// update callbacks that modify the ECM. + private: ComponentInspector *inspector{nullptr}; + }; +} +} +#endif diff --git a/src/gui/plugins/component_inspector/Lidar.qml b/src/gui/plugins/component_inspector/Lidar.qml new file mode 100644 index 0000000000..229ffa7f8a --- /dev/null +++ b/src/gui/plugins/component_inspector/Lidar.qml @@ -0,0 +1,451 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/ComponentInspector" +import "qrc:/qml" + +// Item displaying lidar sensor information. +Rectangle { + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + // Noise mean value + property double noiseMeanValue: model.data[0] + + // Noise mean bias + property double noiseMeanBias: model.data[1] + + // Noise standard deviation value + property double noieStdDevValue: model.data[2] + + // Noise standard deviation bias + property double noiseStdDevBias: model.data[3] + + // Noise dynamic bias standard deviation + property double noiseDynamicBiasStdDev: model.data[4] + + // Noise dynamic bias correlation time + property double noiseDynamicBiasCorrelationTime: model.data[5] + + // Horizontal scan samples + property double horizontalScanSamples: model.data[6] + + // Horizontal scan resolution + property double horizontalScanResolution: model.data[7] + + // Horizontal scan min angle + property double horizontalScanMinAngle: model.data[8] + + // Horizontal scan max angle + property double horizontalScanMaxAngle: model.data[9] + + // Vertical scan samples + property double verticalScanSamples: model.data[10] + + // Vertical scan resolution + property double verticalScanResolution: model.data[11] + + // Vertical scan min angle + property double verticalScanMinAngle: model.data[12] + + // Vertical scan max angle + property double verticalScanMaxAngle: model.data[13] + + // Range min + property double rangeMin: model.data[14] + + // Range max + property double rangeMax: model.data[15] + + // Range resolution + property double rangeResolution: model.data[16] + + // Callback when range min is changed + function onRangeMin(_value) { + rangeMin = _value + LidarImpl.OnLidarChange(rangeMin, rangeMax, rangeResolution, + horizontalScanSamples, horizontalScanResolution, + horizontalScanMinAngle, horizontalScanMaxAngle, + verticalScanSamples, verticalScanResolution, + verticalScanMinAngle, verticalScanMaxAngle) + } + + // Callback when range max is changed + function onRangeMax(_value) { + rangeMax = _value + LidarImpl.OnLidarChange(rangeMin, rangeMax, rangeResolution, + horizontalScanSamples, horizontalScanResolution, + horizontalScanMinAngle, horizontalScanMaxAngle, + verticalScanSamples, verticalScanResolution, + verticalScanMinAngle, verticalScanMaxAngle) + } + + // Callback when range resolution is changed + function onRangeResolution(_value) { + rangeResolution = _value + LidarImpl.OnLidarChange(rangeMin, rangeMax, rangeResolution, + horizontalScanSamples, horizontalScanResolution, + horizontalScanMinAngle, horizontalScanMaxAngle, + verticalScanSamples, verticalScanResolution, + verticalScanMinAngle, verticalScanMaxAngle) + } + + function onHorizontalScan(_samples, _resolution, _minAngle, _maxAngle) { + horizontalScanSamples = _samples + horizontalScanResolution = _resolution + horizontalScanMinAngle = _minAngle + horizontalScanMaxAngle = _maxAngle + LidarImpl.OnLidarChange(rangeMin, rangeMax, rangeResolution, + horizontalScanSamples, horizontalScanResolution, + horizontalScanMinAngle, horizontalScanMaxAngle, + verticalScanSamples, verticalScanResolution, + verticalScanMinAngle, verticalScanMaxAngle) + } + + function onVerticalScan(_samples, _resolution, _minAngle, _maxAngle) { + verticalScanSamples = _samples + verticalScanResolution = _resolution + verticalScanMinAngle = _minAngle + verticalScanMaxAngle = _maxAngle + LidarImpl.OnLidarChange(rangeMin, rangeMax, rangeResolution, + horizontalScanSamples, horizontalScanResolution, + horizontalScanMinAngle, horizontalScanMaxAngle, + verticalScanSamples, verticalScanResolution, + verticalScanMinAngle, verticalScanMaxAngle) + } + + Column { + anchors.fill: parent + + // The expanding header. Make sure that the content to expand has an id set + // to the value "content". + ExpandingTypeHeader { + id: header + + // Set the 'expandingHeaderText' value to override the default header + // values, which is based on the model. + expandingHeaderText: "Lidar" + expandingHeaderToolTip: "Lidar properties" + } + + // This is the content that will be expanded/contracted using the + // ExpandingHeader above. Note the "id: content" attribute. + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? layout.height : 0 + clip: true + color: "transparent" + Layout.leftMargin: 4 + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + ColumnLayout { + id: layout + width: parent.width + + // Space out the section header. + Rectangle { + id: rangeTextRect + width: parent.width + height: childrenRect.height + Layout.leftMargin: 10 + Layout.topMargin: 10 + color: "transparent" + + Text { + text: "Range" + color: "dimgrey" + font.pointSize: 12 + } + } + // Range properties + Rectangle { + id: rangeRect + width: parent.width + Layout.fillWidth: true + height: rangeGrid.height + color: "transparent" + Layout.leftMargin: 20 + + GridLayout { + id: rangeGrid + width: parent.width + columns: 4 + + // Padding + Rectangle { + Layout.columnSpan: 4 + height: 4 + } + + // Range min + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: rangeMinText.width + + Text { + id : rangeMinText + text: 'Min (m)' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: minRangeMa.containsMouse + delay: tooltipDelay + text: "Minimum range distance in meters" + y: rangeMinText.y + 30 + enter: null + exit: null + } + MouseArea { + id: minRangeMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: rangeMinSpin + Layout.fillWidth: true + height: 40 + numberValue: rangeMin + minValue: 0 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onLidarUpdate signal in Noise.qml + Component.onCompleted: { + rangeMinSpin.onChange.connect(onRangeMin) + } + } + // End of min range + + // Max range + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: rangeMaxText.width + + Text { + id : rangeMaxText + text: 'Max (m)' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: rangeMaxMa.containsMouse + delay: tooltipDelay + text: "Maximum range distance in meters" + y: rangeMaxText.y + 30 + enter: null + exit: null + } + MouseArea { + id: rangeMaxMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: rangeMaxSpin + Layout.fillWidth: true + height: 40 + numberValue: rangeMax + minValue: 0 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + rangeMaxSpin.onChange.connect(onRangeMax) + } + } + // End of range max + + // Range resolution + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: rangeResolutionText.width + + Text { + id : rangeResolutionText + text: 'Resolution' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: rangeResolutionMa.containsMouse + delay: tooltipDelay + text: "Range resolution" + y: rangeResolutionText.y + 30 + enter: null + exit: null + } + MouseArea { + id: rangeResolutionMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: rangeResolutionSpin + Layout.fillWidth: true + height: 40 + numberValue: rangeResolution + minValue: 0 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onLidarUpdate signal in Noise.qml + Component.onCompleted: { + rangeResolutionSpin.onChange.connect(onRangeResolution) + } + } + // End of range resolution + } + } + + // Space out the section header. + Rectangle { + id: horizontalTextRect + width: parent.width + height: childrenRect.height + Layout.leftMargin: 10 + Layout.topMargin: 10 + color: "transparent" + + Text { + text: "Horizontal scan" + color: "dimgrey" + font.pointSize: 12 + } + } + Rectangle { + Layout.fillWidth: true + height: horizontalScan.height + Layout.leftMargin: 20 + color: "transparent" + LidarScan { + id: horizontalScan + width: parent.width + + sampleValue: horizontalScanSamples + resolutionValue: horizontalScanResolution + minAngleValue: horizontalScanMinAngle + maxAngleValue: horizontalScanMaxAngle + + // Connect to the onChange signal in Noise.qml + Component.onCompleted: { + horizontalScan.onChange.connect(onHorizontalScan) + } + } + } + + // Space out the section header. + Rectangle { + id: verticalTextRect + width: parent.width + height: childrenRect.height + Layout.leftMargin: 10 + Layout.topMargin: 10 + color: "transparent" + + Text { + text: "Vertical scan" + color: "dimgrey" + font.pointSize: 12 + } + } + Rectangle { + Layout.fillWidth: true + height: verticalScan.height + Layout.leftMargin: 20 + color: "transparent" + + LidarScan { + id: verticalScan + width: parent.width + + sampleValue: verticalScanSamples + resolutionValue: verticalScanResolution + minAngleValue: verticalScanMinAngle + maxAngleValue: verticalScanMaxAngle + + // Connect to the onChange signal in Noise.qml + Component.onCompleted: { + verticalScan.onChange.connect(onVerticalScan) + } + } + } + + // Space out the section header. + Rectangle { + id: linearAccelerationXNoiseTextRect + width: parent.width + height: childrenRect.height + Layout.leftMargin: 10 + Layout.topMargin: 10 + color: "transparent" + + Text { + text: "Noise" + color: "dimgrey" + font.pointSize: 12 + } + } + + // Show the noise values. + Noise { + id: noise + Layout.fillWidth: true + Layout.leftMargin: 20 + + meanValue: noiseMeanValue + meanBias: noiseMeanBias + stdDevValue: noieStdDevValue + stdDevBias: noiseStdDevBias + dynamicBiasStdDev: noiseDynamicBiasStdDev + dynamicBiasCorrelationTime: noiseDynamicBiasCorrelationTime + + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + noise.onNoiseUpdate.connect( + LidarImpl.OnLidarNoise) + } + } + } + } + } +} diff --git a/src/gui/plugins/component_inspector/LidarScan.qml b/src/gui/plugins/component_inspector/LidarScan.qml new file mode 100644 index 0000000000..9289963942 --- /dev/null +++ b/src/gui/plugins/component_inspector/LidarScan.qml @@ -0,0 +1,245 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/ComponentInspector" +import "qrc:/qml" + + +// A component for displaying and editing a lidar scan. +Rectangle { + width: parent.width + height: grid.height + color: "transparent" + + property int sampleValue: horizontalScanSamples + property double resolutionValue: horizontalScanResolution + property double minAngleValue: horizontalScanMinAngle + property double maxAngleValue: horizontalScanMaxAngle + + // onChange signal + signal onChange(int _samples, double _resolution, double _minAngle, double _maxAngle) + + function onSample(_value) { + sampleValue = _value + onChange(sampleValue, resolutionValue, minAngleValue, maxAngleValue) + } + + function onResolution(_value) { + resolutionValue = _value + onChange(sampleValue, resolutionValue, minAngleValue, maxAngleValue) + } + + function onMinAngle(_value) { + minAngleValue = _value + onChange(sampleValue, resolutionValue, minAngleValue, maxAngleValue) + } + + function onMaxAngle(_value) { + maxAngleValue = _value + onChange(sampleValue, resolutionValue, minAngleValue, maxAngleValue) + } + + GridLayout { + id: grid + width: parent.width + columns: 4 + + // Padding + Rectangle { + Layout.columnSpan: 4 + height: 4 + } + + // Samples + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: sampleText.width + + Text { + id : sampleText + text: 'Samples' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: sampleMa.containsMouse + delay: tooltipDelay + text: "Sample count" + y: sampleText.y + 30 + enter: null + exit: null + } + MouseArea { + id: sampleMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: sampleSpin + Layout.fillWidth: true + height: 40 + numberValue: sampleValue + minValue: 0 + maxValue: 100000 + stepValue: 1 + // Send the change signal + Component.onCompleted: { + sampleSpin.onChange.connect(onSample) + } + } + // End of samples + + // Resolution + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: resolutionText.width + + Text { + id : resolutionText + text: 'Resolution' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: resolutionMa.containsMouse + delay: tooltipDelay + text: "This number is multiplied by samples to determine the number of range data points returned." + y: resolutionText.y + 30 + enter: null + exit: null + } + MouseArea { + id: resolutionMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: resolutionSpin + Layout.fillWidth: true + height: 40 + numberValue: resolutionValue + minValue: 0 + maxValue: 100000 + stepValue: 0.1 + // Send the change signal + Component.onCompleted: { + resolutionSpin.onChange.connect(onResolution) + } + } + // End of resolution + + // Min angle + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: minAngleText.width + + Text { + id : minAngleText + text: 'Min angle' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: minAngleMa.containsMouse + delay: tooltipDelay + text: "Minimun angle" + y: minAngleText.y + 30 + enter: null + exit: null + } + MouseArea { + id: minAngleMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: minAngleSpin + Layout.fillWidth: true + height: 40 + numberValue: minAngleValue + minValue: -3.1415 + maxValue: 3.1415 + stepValue: 0.1 + // Send the change signal + Component.onCompleted: { + minAngleSpin.onChange.connect(onMinAngle) + } + } + // End of min angle + + // Max angle + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: maxAngleText.width + + Text { + id : maxAngleText + text: 'Max angle' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: maxAngleMa.containsMouse + delay: tooltipDelay + text: "Maximum angle" + y: maxAngleText.y + 30 + enter: null + exit: null + } + MouseArea { + id: maxAngleMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: maxAngleSpin + Layout.fillWidth: true + height: 40 + numberValue: maxAngleValue + minValue: -3.1415 + maxValue: 3.1415 + stepValue: 0.1 + // Connect to the onLidarUpdate signal in Noise.qml + Component.onCompleted: { + maxAngleSpin.onChange.connect(onMaxAngle) + } + } + // End of max angle + } +} diff --git a/src/gui/plugins/component_inspector/Noise.qml b/src/gui/plugins/component_inspector/Noise.qml index f2a39d74de..63cb50d8d2 100644 --- a/src/gui/plugins/component_inspector/Noise.qml +++ b/src/gui/plugins/component_inspector/Noise.qml @@ -145,7 +145,7 @@ Rectangle { visible: meanMa.containsMouse delay: tooltipDelay text: "Mean value" - y: noise.y - 30 + y: meanText.y + 30 enter: null exit: null } @@ -188,7 +188,7 @@ Rectangle { visible: meanBiasMa.containsMouse delay: tooltipDelay text: "Mean bias" - y: noise.y - 30 + y: meanBiasText.y + 30 enter: null exit: null } @@ -246,7 +246,7 @@ Rectangle { visible: stdDevMa.containsMouse delay: tooltipDelay text: "Standard deviation value" - y: noise.y - 30 + y: stddevText.y + 30 enter: null exit: null } @@ -289,7 +289,7 @@ Rectangle { visible: stddevBiasTextMa.containsMouse delay: tooltipDelay text: "Standard deviation bias" - y: noise.y - 30 + y: stddevBiasText.y + 30 enter: null exit: null } @@ -347,7 +347,7 @@ Rectangle { visible: dynamicBiasStddevTextMa.containsMouse delay: tooltipDelay text: "Standard deviation" - y: noise.y - 30 + y: dynamicBiasStddevText.y + 30 enter: null exit: null } @@ -392,7 +392,7 @@ Rectangle { visible: dynamicBiasCorrelationTimeTextMa.containsMouse delay: tooltipDelay text: "Correlation time in seconds." - y: noise.y - 30 + y: dynamicBiasCorrelationTimeText.y + 30 enter: null exit: null } diff --git a/src/gui/plugins/component_inspector/StateAwareSpin.qml b/src/gui/plugins/component_inspector/StateAwareSpin.qml index 33db4d3e73..10c253f84f 100644 --- a/src/gui/plugins/component_inspector/StateAwareSpin.qml +++ b/src/gui/plugins/component_inspector/StateAwareSpin.qml @@ -56,7 +56,7 @@ Rectangle { stepSize: stepValue decimals: { writableSpin.value = writableSpin.activeFocus ? writableSpin.value : numberValue - componentInspector.getDecimals(writableSpin.width) + stepSize == 1 ? 0 : componentInspector.getDecimals(writableSpin.width) } onEditingFinished: { numberValue = writableSpin.value From d392d0fc3448d54334550fc171c97c7f94806393 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 30 Nov 2021 18:40:19 -0600 Subject: [PATCH 2/2] Model Editor: Add Joints to model (#1196) * Model Editor: Add Joints to model Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Style and documentation Signed-off-by: Nate Koenig * Suppress physics warnings on newly-created joints Signed-off-by: Michael Carroll * Added a header Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/ignition/gazebo/SdfEntityCreator.hh | 7 + include/ignition/gazebo/gui/GuiEvents.hh | 14 +- src/SdfEntityCreator.cc | 63 +++++-- .../component_inspector/ComponentInspector.cc | 58 +++++- .../component_inspector/ComponentInspector.hh | 27 +++ .../ComponentInspector.qml | 165 +++++++++++++++++- .../component_inspector/ModelEditor.cc | 125 ++++++++++--- src/systems/physics/Physics.cc | 19 ++ 8 files changed, 419 insertions(+), 59 deletions(-) diff --git a/include/ignition/gazebo/SdfEntityCreator.hh b/include/ignition/gazebo/SdfEntityCreator.hh index e5c2442279..c54f8effe3 100644 --- a/include/ignition/gazebo/SdfEntityCreator.hh +++ b/include/ignition/gazebo/SdfEntityCreator.hh @@ -122,6 +122,13 @@ namespace ignition /// \return Joint entity. public: Entity CreateEntities(const sdf::Joint *_joint); + /// \brief Create all entities that exist in the sdf::Joint object and + /// load their plugins. + /// \param[in] _joint SDF joint object. + /// \param[in] _resolved True if all frames are already resolved + /// \return Joint entity. + public: Entity CreateEntities(const sdf::Joint *_joint, bool _resolved); + /// \brief Create all entities that exist in the sdf::Visual object and /// load their plugins. /// \param[in] _visual SDF visual object. diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index ee0e49e2f2..353a85e805 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_GUI_GUIEVENTS_HH_ #include +#include #include #include @@ -191,8 +192,8 @@ namespace events /// \brief Constructor /// \param[in] _tranformModeActive is the transform control mode active public: explicit ModelEditorAddEntity(QString _entity, QString _type, - ignition::gazebo::Entity _parent, QString _uri) : - QEvent(kType), entity(_entity), type(_type), parent(_parent), uri(_uri) + ignition::gazebo::Entity _parent) : + QEvent(kType), entity(_entity), type(_type), parent(_parent) { } @@ -202,12 +203,6 @@ namespace events return this->entity; } - /// \brief Get the URI, if any, associated with the entity to add - public: QString Uri() const - { - return this->uri; - } - /// \brief Get the entity type public: QString EntityType() const { @@ -220,13 +215,12 @@ namespace events return this->parent; } - /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 7); + public : QMap data; private: QString entity; private: QString type; private: ignition::gazebo::Entity parent; - private: QString uri; }; } // namespace events diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 11db16cbb5..17c8155765 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -588,6 +588,13 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) ////////////////////////////////////////////////// Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint) +{ + return this->CreateEntities(_joint, false); +} + +////////////////////////////////////////////////// +Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint, + bool _resolved) { IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Joint)"); @@ -645,34 +652,54 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint) this->dataPtr->ecm->CreateComponent(jointEntity , components::ThreadPitch(_joint->ThreadPitch())); + std::string resolvedParentLinkName; - const auto resolveParentErrors = - _joint->ResolveParentLink(resolvedParentLinkName); - if (!resolveParentErrors.empty()) + if (_resolved) { - ignerr << "Failed to resolve parent link for joint '" << _joint->Name() - << "' with parent name '" << _joint->ParentLinkName() << "'" - << std::endl; + resolvedParentLinkName = _joint->ParentLinkName(); + } + else + { + + const auto resolveParentErrors = + _joint->ResolveParentLink(resolvedParentLinkName); + if (!resolveParentErrors.empty()) + { + ignerr << "Failed to resolve parent link for joint '" << _joint->Name() + << "' with parent name '" << _joint->ParentLinkName() << "'" + << std::endl; + for (const auto &error : resolveParentErrors) + { + ignerr << error << std::endl; + } - return kNullEntity; + return kNullEntity; + } } this->dataPtr->ecm->CreateComponent( jointEntity, components::ParentLinkName(resolvedParentLinkName)); std::string resolvedChildLinkName; - const auto resolveChildErrors = - _joint->ResolveChildLink(resolvedChildLinkName); - if (!resolveChildErrors.empty()) - { - ignerr << "Failed to resolve child link for joint '" << _joint->Name() - << "' with child name '" << _joint->ChildLinkName() << "'" - << std::endl; - for (const auto &error : resolveChildErrors) + if (_resolved) + { + resolvedChildLinkName = _joint->ChildLinkName(); + } + else + { + const auto resolveChildErrors = + _joint->ResolveChildLink(resolvedChildLinkName); + if (!resolveChildErrors.empty()) { - ignerr << error << std::endl; - } + ignerr << "Failed to resolve child link for joint '" << _joint->Name() + << "' with child name '" << _joint->ChildLinkName() << "'" + << std::endl; + for (const auto &error : resolveChildErrors) + { + ignerr << error << std::endl; + } - return kNullEntity; + return kNullEntity; + } } this->dataPtr->ecm->CreateComponent( diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index eaf2468e93..ebeab34487 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -103,6 +103,9 @@ namespace ignition::gazebo /// \brief Nested model or not public: bool nestedModel = false; + /// \brief If a model, keep track of available links + public: QStringList modelLinks = {}; + /// \brief Whether currently locked on a given entity public: bool locked{false}; @@ -497,6 +500,25 @@ void ComponentInspector::Update(const UpdateInfo &_info, } this->NestedModelChanged(); + // Get available links for the model. + this->dataPtr->modelLinks.clear(); + this->dataPtr->modelLinks.append("world"); + _ecm.EachNoCache< + components::Name, + components::Link, + components::ParentEntity>([&](const ignition::gazebo::Entity &, + const components::Name *_name, + const components::Link *, + const components::ParentEntity *_parent) -> bool + { + if (_parent->Data() == this->dataPtr->entity) + { + this->dataPtr->modelLinks.push_back( + QString::fromStdString(_name->Data())); + } + return true; + }); + this->ModelLinksChanged(); continue; } @@ -1125,6 +1147,19 @@ bool ComponentInspector::NestedModel() const return this->dataPtr->nestedModel; } +///////////////////////////////////////////////// +void ComponentInspector::SetModelLinks(const QStringList &_modelLinks) +{ + this->dataPtr->modelLinks = _modelLinks; + this->ModelLinksChanged(); +} + +///////////////////////////////////////////////// +QStringList ComponentInspector::ModelLinks() const +{ + return this->dataPtr->modelLinks; +} + ///////////////////////////////////////////////// void ComponentInspector::OnAddEntity(const QString &_entity, const QString &_type) @@ -1132,7 +1167,23 @@ void ComponentInspector::OnAddEntity(const QString &_entity, // currently just assumes parent is the model // todo(anyone) support adding visuals / collisions / sensors to links ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( - _entity, _type, this->dataPtr->entity, QString("")); + _entity, _type, this->dataPtr->entity); + + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &addEntityEvent); +} + +///////////////////////////////////////////////// +void ComponentInspector::OnAddJoint(const QString &_jointType, + const QString &_parentLink, + const QString &_childLink) +{ + ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( + _jointType, "joint", this->dataPtr->entity); + + addEntityEvent.data.insert("parent_link", _parentLink); + addEntityEvent.data.insert("child_link", _childLink); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), @@ -1157,7 +1208,10 @@ void ComponentInspector::OnLoadMesh(const QString &_entity, } ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( - _entity, _type, this->dataPtr->entity, QString(meshStr.c_str())); + _entity, _type, this->dataPtr->entity); + + addEntityEvent.data.insert("uri", QString(meshStr.c_str())); + ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &addEntityEvent); diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 7f756de233..25e38aead9 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -185,6 +185,14 @@ namespace gazebo NOTIFY TypeChanged ) + /// \brief Type + Q_PROPERTY( + QStringList modelLinks + READ ModelLinks + WRITE SetModelLinks + NOTIFY ModelLinksChanged + ) + /// \brief Locked Q_PROPERTY( bool locked @@ -355,6 +363,25 @@ namespace gazebo public: Q_INVOKABLE void OnAddEntity(const QString &_entity, const QString &_type); + /// \brief Callback in Qt thread when a joint is to be added + /// \param[in] _jointType Type of joint to add (revolute, fixed, etc) + /// \param[in] _parentLink Name of the link to be the parent link + /// \param[in] _childLink Name of the link to be the child link + public: Q_INVOKABLE void OnAddJoint(const QString &_jointType, + const QString &_parentLink, + const QString &_childLink); + + /// \brief Return the list of availabe links if a model is selected. + /// \return List of available links. + public: Q_INVOKABLE QStringList ModelLinks() const; + + /// \brief Set the list of availabe links when a model is selected. + /// \param[in] _modelLinks List of available links. + public: Q_INVOKABLE void SetModelLinks(const QStringList &_modelLinks); + + /// \brief Notify that locked has changed. + signals: void ModelLinksChanged(); + /// \brief Callback to insert a new entity /// \param[in] _entity Entity to add, e.g. box, sphere, cylinder, etc /// \param[in] _type Entity type, e.g. link, visual, collision, etc diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 4d6d8b41b4..ea95e6e838 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -157,13 +157,64 @@ Rectangle { height: childrenRect.height Text { - text: sectionText + text: sectionText font.pointSize: 10 padding: 5 } } } + Dialog { + id: jointDialog + modal: true + focus: true + header: ColumnLayout { + id: jointAddHeader + Text { + text:"Add joint" + font.pointSize: 14 + padding: 20 + } + + Text { + text:"Select the parent and child links" + font.pointSize: 12 + leftPadding: 20 + rightPadding: 20 + bottomPadding: 20 + } + } + + standardButtons: Dialog.Ok | Dialog.Cancel + + property string jointType: "empty" + + contentItem: ColumnLayout { + Text { + id: parentBoxText + text: "Parent Link" + } + ComboBox { + id: parentBox + model: ComponentInspector.modelLinks + currentIndex: 0 + } + Text { + id: childBoxText + text: "Child Link" + } + ComboBox { + id: childBox + model: ComponentInspector.modelLinks + currentIndex: 1 + } + } + + onAccepted: { + ComponentInspector.OnAddJoint(jointType, parentBox.currentText, childBox.currentText) + } + } + Rectangle { id: header height: lockButton.height @@ -283,6 +334,7 @@ Rectangle { } } + Menu { id: addLinkMenu @@ -400,7 +452,116 @@ Rectangle { } } - // \todo(anyone) Add joints + MenuSeparator { + padding: 0 + topPadding: 12 + bottomPadding: 12 + contentItem: Rectangle { + implicitWidth: 200 + implicitHeight: 1 + color: "#1E000000" + } + } + + Item { + Layout.fillWidth: true + height: childrenRect.height + Loader { + property string sectionText: "Joint" + sourceComponent: menuSectionHeading + } + } + + MenuItem { + id: revoluteJoint + text: "Revolute" + onClicked: { + jointDialog.jointType = "revolute" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: ballJoint + text: "Ball" + onClicked: { + jointDialog.jointType = "ball" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: continuousJoint + text: "Continuous" + onClicked: { + jointDialog.jointType = "continuous" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: fixedJoint + text: "Fixed" + onClicked: { + jointDialog.jointType = "fixed" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: gearboxJoint + text: "Gearbox" + onClicked: { + jointDialog.jointType = "gearbox" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: prismaticJoint + text: "Prismatic" + onClicked: { + jointDialog.jointType = "prismatic" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: revolute2Joint + text: "Revolute2" + onClicked: { + jointDialog.jointType = "revolute2" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: screwJoint + text: "Screw" + onClicked: { + jointDialog.jointType = "screw" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: universalJoint + text: "Universal" + onClicked: { + jointDialog.jointType = "universal" + jointDialog.open() + addLinkMenu.close() + } + } + } } diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index a40e8117da..2fec07dbc5 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -19,7 +19,10 @@ #include #include #include +#include +#include #include + #include #include #include @@ -53,8 +56,8 @@ namespace ignition::gazebo /// \brief Parent entity to add the entity to public: Entity parentEntity; - /// \brief Entity URI, such as a URI for a mesh. - public: std::string uri; + /// \brief Additional entity-specific data needed + public: std::unordered_map data; }; class ModelEditorPrivate @@ -64,11 +67,10 @@ namespace ignition::gazebo /// directional, etc /// \param[in] _entityType Type of entity: link, visual, collision, etc /// \param[in] _parentEntity Name of parent entity - /// \param[in] _uri URI associated with the entity, needed for mesh - /// types. + /// \param[in] _data Additional variable data for specific instances public: void HandleAddEntity(const std::string &_geomOrLightType, const std::string &_entityType, Entity _parentEntity, - const std::string &_uri); + const std::unordered_map &_data); /// \brief Get a SDF string of a geometry /// \param[in] _eta Entity to add. @@ -82,6 +84,10 @@ namespace ignition::gazebo /// \param[in] _eta Entity to add. public: std::string LinkSDFString(const EntityToAdd &_eta) const; + /// \brief Get a SDF string of a link + /// \param[in] _eta Entity to add. + public: sdf::ElementPtr JointSDF(const EntityToAdd &_eta) const; + /// \brief Entity Creator API. public: std::unique_ptr entityCreator{nullptr}; @@ -134,12 +140,20 @@ void ModelEditor::Update(const UpdateInfo &, std::lock_guard lock(this->dataPtr->mutex); // add link entities to the ECM + std::list entities; std::set newEntities; + for (const auto &eta : this->dataPtr->entitiesToAdd) { - sdf::Link linkSdf; + if (eta.parentEntity == kNullEntity) + { + ignerr << "Parent entity not defined." << std::endl; + continue; + } + if (eta.entityType == "link") { + sdf::Link linkSdf; // create an sdf::Link to it can be added to the ECM throught the // CreateEntities call std::string linkSDFStr = this->dataPtr->LinkSDFString(eta); @@ -157,11 +171,6 @@ void ModelEditor::Update(const UpdateInfo &, { continue; } - if (eta.parentEntity == kNullEntity) - { - ignerr << "Parent entity not defined." << std::endl; - continue; - } // generate unique link name // note passing components::Link() as arg to EntityByComponents causes @@ -188,24 +197,54 @@ void ModelEditor::Update(const UpdateInfo &, // traverse the tree and add all new entities created by the entity // creator to the set - std::list entities; entities.push_back(entity); - while (!entities.empty()) + } + else if (eta.entityType == "joint") + { + sdf::ElementPtr jointElem = this->dataPtr->JointSDF(eta); + if (nullptr == jointElem) + continue; + + std::string jointName = "joint"; + Entity jointEnt = _ecm.EntityByComponents( + components::ParentEntity(eta.parentEntity), + components::Name(jointName)); + int64_t counter = 0; + while (jointEnt) { - Entity ent = entities.front(); - entities.pop_front(); + jointName = std::string("joint") + "_" + std::to_string(++counter); + jointEnt = _ecm.EntityByComponents( + components::ParentEntity(eta.parentEntity), + components::Name(jointName)); + } - // add new entity created - newEntities.insert(ent); + jointElem->SetName(jointName); - auto childEntities = _ecm.EntitiesByComponents( - components::ParentEntity(ent)); - for (const auto &child : childEntities) - entities.push_back(child); - } + sdf::Joint jointSdf; + jointSdf.Load(jointElem); + auto entity = + this->dataPtr->entityCreator->CreateEntities(&jointSdf, true); + this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity); + entities.push_back(entity); } } + // traverse the tree and add all new entities created by the entity + // creator to the set + while (!entities.empty()) + { + Entity ent = entities.front(); + entities.pop_front(); + + // add new entity created + newEntities.insert(ent); + + auto childEntities = _ecm.EntitiesByComponents( + components::ParentEntity(ent)); + for (const auto &child : childEntities) + entities.push_back(child); + } + // use tmp AddedRemovedEntities event to update other gui plugins // note this event will be removed in Ignition Garden std::set removedEntities; @@ -226,10 +265,16 @@ bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) auto event = reinterpret_cast(_event); if (event) { + // Convert to an unordered map of STL strings for convenience + std::unordered_map data; + for (auto key : event->data.toStdMap()) + { + data[key.first.toStdString()] = key.second.toStdString(); + } + this->dataPtr->HandleAddEntity(event->Entity().toStdString(), event->EntityType().toStdString(), - event->ParentEntity(), - event->Uri().toStdString()); + event->ParentEntity(), data); } } @@ -340,7 +385,7 @@ std::string ModelEditorPrivate::GeomSDFString(const EntityToAdd &_eta) const { geomStr << "" - << " " << _eta.uri << "" + << " " << _eta.data.at("uri") << "" << ""; } else @@ -355,6 +400,31 @@ std::string ModelEditorPrivate::GeomSDFString(const EntityToAdd &_eta) const return geomStr.str(); } +///////////////////////////////////////////////// +sdf::ElementPtr ModelEditorPrivate::JointSDF(const EntityToAdd &_eta) const +{ + std::unordered_set validJointTypes = { + "revolute", "ball", "continuous", "fixed", "gearbox", "prismatic", + "revolute2", "screw", "universal"}; + + if (validJointTypes.count(_eta.geomOrLightType) == 0) + { + ignwarn << "Joint type not supported: " + << _eta.geomOrLightType << std::endl; + return nullptr; + } + + auto joint = std::make_shared(); + sdf::initFile("joint.sdf", joint); + + joint->GetAttribute("name")->Set("joint"); + joint->GetAttribute("type")->Set(_eta.geomOrLightType); + joint->GetElement("parent")->Set(_eta.data.at("parent_link")); + joint->GetElement("child")->Set(_eta.data.at("child_link")); + + return joint; +} + ///////////////////////////////////////////////// std::string ModelEditorPrivate::LinkSDFString(const EntityToAdd &_eta) const { @@ -398,7 +468,7 @@ std::string ModelEditorPrivate::LinkSDFString(const EntityToAdd &_eta) const ///////////////////////////////////////////////// void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, const std::string &_type, Entity _parentEntity, - const std::string &_uri) + const std::unordered_map &_data) { std::lock_guard lock(this->mutex); std::string entType = common::lowercase(_type); @@ -408,6 +478,7 @@ void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, eta.entityType = entType; eta.geomOrLightType = geomLightType; eta.parentEntity = _parentEntity; - eta.uri = _uri; + eta.data = _data; + this->entitiesToAdd.push_back(eta); } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index f2414936ba..dcaeb8ef30 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -621,6 +621,12 @@ class ignition::gazebo::systems::PhysicsPrivate /// of links and collision elements. This also lets us suppress some /// invalid error messages. public: std::set linkAddedToModel; + + /// \brief Set of joints that were added to an existing model. This set + /// is used to track joints that were added to an existing model, such as + /// through the GUI model editor, so that we can avoid premature creation + /// of joints. This also lets us suppress some invalid error messages. + public: std::set jointAddedToModel; }; ////////////////////////////////////////////////// @@ -789,6 +795,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) { // Clear the set of links that were added to a model. this->linkAddedToModel.clear(); + this->jointAddedToModel.clear(); this->CreateWorldEntities(_ecm); this->CreateModelEntities(_ecm); @@ -1301,6 +1308,18 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) const components::ParentLinkName *_parentLinkName, const components::ChildLinkName *_childLinkName) -> bool { + // If the parent model is scheduled for recreation, then do not + // try to create a new link. This situation can occur when a link + // is added to a model from the GUI model editor. + if (_ecm.EntityHasComponentType(_parentModel->Data(), + components::Recreate::typeId)) + { + // Add this entity to the set of newly added links to existing + // models. + this->jointAddedToModel.insert(_entity); + return true; + } + // Check if joint already exists if (this->entityJointMap.HasEntity(_entity)) {