From b67c6143955820f212d0083fbb5ba7199948e7d3 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 15 Jul 2021 12:00:12 -0700 Subject: [PATCH 01/10] First iteration of the plot plugin, still need to work on usability Signed-off-by: Louise Poubel --- include/ignition/gazebo/Util.hh | 8 + src/Util.cc | 46 +++- src/Util_TEST.cc | 31 +++ src/gui/plugins/CMakeLists.txt | 1 + src/gui/plugins/plot_3d/CMakeLists.txt | 6 + src/gui/plugins/plot_3d/Plot3D.cc | 346 +++++++++++++++++++++++++ src/gui/plugins/plot_3d/Plot3D.hh | 142 ++++++++++ src/gui/plugins/plot_3d/Plot3D.qml | 308 ++++++++++++++++++++++ src/gui/plugins/plot_3d/Plot3D.qrc | 5 + 9 files changed, 892 insertions(+), 1 deletion(-) create mode 100644 src/gui/plugins/plot_3d/CMakeLists.txt create mode 100644 src/gui/plugins/plot_3d/Plot3D.cc create mode 100644 src/gui/plugins/plot_3d/Plot3D.hh create mode 100644 src/gui/plugins/plot_3d/Plot3D.qml create mode 100644 src/gui/plugins/plot_3d/Plot3D.qrc diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 8dc1958585..635d744bdf 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -51,6 +51,14 @@ namespace ignition const EntityComponentManager &_ecm, const std::string &_delim = "/", bool _includePrefix = true); + /// \brief Helper function to get an entity given its scoped name. + /// \param[in] _scopedName Entity's scoped name. + /// \param[in] _ecm Immutable reference to ECM. + /// \param[in] _delim Delimiter between names, defaults to "::". + Entity IGNITION_GAZEBO_VISIBLE entityFromScopedName( + const std::string &_entity, const EntityComponentManager &_ecm, + const std::string &_delim = "::"); + /// \brief Generally, each entity will be of some specific high-level type, /// such as World, Sensor, Collision, etc, and one type only. /// The entity type is usually marked by having some component that diff --git a/src/Util.cc b/src/Util.cc index af876c259d..29102f0cbc 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -59,8 +59,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { math::Pose3d worldPose(const Entity &_entity, const EntityComponentManager &_ecm) { + auto poseComp = _ecm.Component(_entity); + if (nullptr == poseComp) + return math::Pose3d(); + // work out pose in world frame - math::Pose3d pose = _ecm.Component(_entity)->Data(); + math::Pose3d pose = poseComp->Data(); auto p = _ecm.Component(_entity); while (p) { @@ -125,6 +129,46 @@ std::string scopedName(const Entity &_entity, return result; } +////////////////////////////////////////////////// +Entity entityFromScopedName(const std::string &_scopedName, + const EntityComponentManager &_ecm, const std::string &_delim) +{ + Entity result{kNullEntity}; + + std::vector names; + size_t pos1 = 0; + size_t pos2 = _scopedName.find(_delim); + while (pos2 != std::string::npos) + { + names.push_back(_scopedName.substr(pos1, pos2 - pos1)); + pos1 = pos2 + _delim.length(); + pos2 = _scopedName.find(_delim, pos2 + _delim.length()); + } + names.push_back(_scopedName.substr(pos1, _scopedName.size()-pos1)); + + for (const auto &name : names) + { + Entity entity; + if (kNullEntity == result) + { + entity = _ecm.EntityByComponents(components::Name(name)); + } + else + { + entity = _ecm.EntityByComponents(components::Name(name), + components::ParentEntity(result)); + } + + if (kNullEntity == entity) + { + return kNullEntity; + } + result = entity; + } + + return result; +} + ////////////////////////////////////////////////// ComponentTypeId entityTypeId(const Entity &_entity, const EntityComponentManager &_ecm) diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 5325be39d2..1bbdfaef18 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -216,6 +216,37 @@ TEST(UtilTest, ScopedName) EXPECT_EQ(worldEntity, gazebo::worldEntity(linkCCEntity, ecm)); EXPECT_EQ(worldEntity, gazebo::worldEntity(actorDEntity, ecm)); EXPECT_EQ(kNullEntity, gazebo::worldEntity(kNullEntity, ecm)); + + // Entity from scoped name + EXPECT_EQ(kNullEntity, gazebo::entityFromScopedName("banana", ecm)); + EXPECT_EQ(kNullEntity, gazebo::entityFromScopedName( + "world_name::banana", ecm)); + EXPECT_EQ(kNullEntity, gazebo::entityFromScopedName( + "modelB_name::banana", ecm)); + + EXPECT_EQ(worldEntity, gazebo::entityFromScopedName( + "world_name", ecm)); + + EXPECT_EQ(modelBEntity, gazebo::entityFromScopedName( + "world_name::modelB_name", ecm)); + EXPECT_EQ(modelBEntity, gazebo::entityFromScopedName( + "modelB_name", ecm)); + + EXPECT_EQ(linkBEntity, gazebo::entityFromScopedName( + "world_name::modelB_name::linkB_name", ecm)); + EXPECT_EQ(linkBEntity, gazebo::entityFromScopedName( + "modelB_name::linkB_name", ecm)); + EXPECT_EQ(linkBEntity, gazebo::entityFromScopedName( + "linkB_name", ecm)); + + EXPECT_EQ(sensorBEntity, gazebo::entityFromScopedName( + "world_name::modelB_name::linkB_name::sensorB_name", ecm)); + EXPECT_EQ(sensorBEntity, gazebo::entityFromScopedName( + "modelB_name::linkB_name::sensorB_name", ecm)); + EXPECT_EQ(sensorBEntity, gazebo::entityFromScopedName( + "linkB_name::sensorB_name", ecm)); + EXPECT_EQ(sensorBEntity, gazebo::entityFromScopedName( + "sensorB_name", ecm)); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 9f68828463..93ee2672fe 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -111,6 +111,7 @@ add_subdirectory(entity_tree) add_subdirectory(grid_config) add_subdirectory(joint_position_controller) add_subdirectory(playback_scrubber) +add_subdirectory(plot_3d) add_subdirectory(resource_spawner) add_subdirectory(scene3d) add_subdirectory(shapes) diff --git a/src/gui/plugins/plot_3d/CMakeLists.txt b/src/gui/plugins/plot_3d/CMakeLists.txt new file mode 100644 index 0000000000..73423d7090 --- /dev/null +++ b/src/gui/plugins/plot_3d/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_gui_plugin(Plot3D + SOURCES + Plot3D.cc + QT_HEADERS + Plot3D.hh +) diff --git a/src/gui/plugins/plot_3d/Plot3D.cc b/src/gui/plugins/plot_3d/Plot3D.cc new file mode 100644 index 0000000000..7581bb87d9 --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.cc @@ -0,0 +1,346 @@ +/* + * 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 +#include + +#include + +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/Util.hh" + +#include "Plot3D.hh" + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ + /// \brief Private data class for Plot3D + class Plot3DPrivate + { + /// \brief Transport node. + public: transport::Node node; + + /// \brief Whether currently locked on a given entity + public: bool locked{false}; + + /// \brief Current target entity. + public: Entity targetEntity{kNullEntity}; + + /// \brief Name of the target entity. + public: std::string targetName; + + /// \brief Whether the target entity has been changed. + public: bool targetEntityDirty{false}; + + /// \brief Whether the target name has been changed. + public: bool targetNameDirty{false}; + + /// \brief Current marker message. + public: msgs::Marker markerMsg; + + /// \brief Marker color. + public: math::Color color{math::Color::Blue}; + + /// \brief Previous plotted position. + public: math::Vector3d prevPos; + + /// \brief Offset from entity origin to place plot point. The orientation + /// isn't used. The offset is expressed in the entity's frame. + public: math::Pose3d offset; + + /// \brief Minimum distance between points. If the object has moved by less + /// than this distance, a new point isn't plotted. + public: double minDistance{0.05}; + + /// \brief Maximum number of points in the plot. When the plot reaches this + /// size, older points are deleted. + public: int64_t maxPoints{1000}; + + /// \brief Protects variables that are updated by the user. + public: std::mutex mutex; + }; +} +} +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +Plot3D::Plot3D() + : GuiSystem(), dataPtr(std::make_unique()) +{ + qRegisterMetaType("Entity"); +} + +///////////////////////////////////////////////// +Plot3D::~Plot3D() = default; + +///////////////////////////////////////////////// +void Plot3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "3D Plot"; + + // Parameters from SDF + if (_pluginElem) + { + auto nameElem = _pluginElem->FirstChildElement("entity_name"); + if (nullptr != nameElem && nullptr != nameElem->GetText()) + { + this->dataPtr->targetName = nameElem->GetText(); + this->dataPtr->targetNameDirty = true; + this->dataPtr->locked = true; + } + + auto offsetElem = _pluginElem->FirstChildElement("offset"); + if (nullptr != offsetElem && nullptr != offsetElem->GetText()) + { + math::Vector3d pos; + std::stringstream offsetStr; + offsetStr << std::string(offsetElem->GetText()); + offsetStr >> pos; + this->dataPtr->offset.Set(pos, math::Vector3d::Zero); + } + + auto colorElem = _pluginElem->FirstChildElement("color"); + if (nullptr != colorElem && nullptr != colorElem->GetText()) + { + std::stringstream colorStr; + colorStr << std::string(colorElem->GetText()); + colorStr >> this->dataPtr->color; + } + + auto distElem = _pluginElem->FirstChildElement("minimum_distance"); + if (nullptr != distElem && nullptr != distElem->GetText()) + { + distElem->QueryDoubleText(&this->dataPtr->minDistance); + } + + auto ptsElem = _pluginElem->FirstChildElement("maximum_points"); + if (nullptr != ptsElem && nullptr != ptsElem->GetText()) + { + ptsElem->QueryInt64Text(&this->dataPtr->maxPoints); + } + } + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +////////////////////////////////////////////////// +void Plot3D::Update(const UpdateInfo &, EntityComponentManager &_ecm) +{ + bool newTarget{false}; + + // New target by name, get entity + if (this->dataPtr->targetNameDirty) + { + auto entity = entityFromScopedName(this->dataPtr->targetName, _ecm); + + if (kNullEntity == entity) + { + // Keep trying + return; + } + + this->dataPtr->targetEntity = entity; + this->dataPtr->targetNameDirty = false; + newTarget = true; + } + + // New target by entity, get name + if (this->dataPtr->targetEntityDirty) + { + this->dataPtr->targetEntityDirty = false; + + auto name = _ecm.ComponentData( + this->dataPtr->targetEntity); + if (!name) + { + this->dataPtr->targetName.clear(); + return; + } + this->dataPtr->targetName = name.value(); + + newTarget = true; + } + + if (newTarget) + { + // Clear previous plot + this->dataPtr->markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER); + this->dataPtr->node.Request("/marker", this->dataPtr->markerMsg); + + // Reset message + this->dataPtr->markerMsg.Clear(); + this->dataPtr->markerMsg.set_ns("plot_" + this->dataPtr->targetName); + this->dataPtr->markerMsg.set_id(this->dataPtr->targetEntity); + this->dataPtr->markerMsg.set_action(msgs::Marker::ADD_MODIFY); + this->dataPtr->markerMsg.set_type(msgs::Marker::LINE_STRIP); + this->dataPtr->markerMsg.set_visibility(msgs::Marker::GUI); + + // Update view + this->TargetEntityChanged(); + this->TargetNameChanged(); + } + + // Get entity pose + auto pose = worldPose(this->dataPtr->targetEntity, _ecm); + + auto point = (pose * this->dataPtr->offset).Pos(); + + // Only add points if the distance is past a threshold. + if (point.Distance(this->dataPtr->prevPos) < this->dataPtr->minDistance) + return; + + this->dataPtr->prevPos = point; + ignition::msgs::Set(this->dataPtr->markerMsg.add_point(), point); + + // Reduce message array + if (this->dataPtr->markerMsg.point_size() > this->dataPtr->maxPoints) + this->dataPtr->markerMsg.mutable_point()->DeleteSubrange(0, 5); + + msgs::Set(this->dataPtr->markerMsg.mutable_material()->mutable_ambient(), + this->dataPtr->color); + msgs::Set(this->dataPtr->markerMsg.mutable_material()->mutable_diffuse(), + this->dataPtr->color); + + // Request +//igndbg << this->dataPtr->markerMsg.DebugString() << std::endl; + this->dataPtr->node.Request("/marker", this->dataPtr->markerMsg); +} + +///////////////////////////////////////////////// +bool Plot3D::eventFilter(QObject *_obj, QEvent *_event) +{ + if (!this->dataPtr->locked) + { + if (_event->type() == gazebo::gui::events::EntitiesSelected::kType) + { + auto event = reinterpret_cast(_event); + if (event && !event->Data().empty()) + { + this->SetTargetEntity(*event->Data().begin()); + } + } + + if (_event->type() == gazebo::gui::events::DeselectAllEntities::kType) + { + auto event = reinterpret_cast( + _event); + if (event) + { + this->SetTargetEntity(kNullEntity); + } + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +Entity Plot3D::TargetEntity() const +{ + return this->dataPtr->targetEntity; +} + +///////////////////////////////////////////////// +void Plot3D::SetTargetEntity(Entity _entity) +{ + this->dataPtr->targetEntity = _entity; + this->dataPtr->targetEntityDirty = true; + this->TargetEntityChanged(); + + if (this->dataPtr->targetEntity == kNullEntity) + { + this->dataPtr->targetName.clear(); + } +} + +///////////////////////////////////////////////// +QString Plot3D::TargetName() const +{ + return QString::fromStdString(this->dataPtr->targetName); +} + +///////////////////////////////////////////////// +void Plot3D::SetTargetName(const QString &_targetName) +{ + this->dataPtr->targetName = _targetName.toStdString(); + this->dataPtr->targetNameDirty = true; + this->TargetNameChanged(); +} + +///////////////////////////////////////////////// +bool Plot3D::Locked() const +{ + return this->dataPtr->locked; +} + +///////////////////////////////////////////////// +void Plot3D::SetLocked(bool _locked) +{ + this->dataPtr->locked = _locked; + this->LockedChanged(); +} + +///////////////////////////////////////////////// +void Plot3D::SetOffset(double _x, double _y, double _z) +{ + this->dataPtr->offset.Set(_x, _y, _z, 0, 0, 0); +} + +///////////////////////////////////////////////// +void Plot3D::SetColor(double _r, double _g, double _b) +{ + this->dataPtr->color.Set(_r, _g, _b); +} + +///////////////////////////////////////////////// +void Plot3D::SetMinDistance(double _dist) +{ + this->dataPtr->minDistance = _dist; +} + +///////////////////////////////////////////////// +void Plot3D::SetMaxPoints(int _max) +{ + this->dataPtr->maxPoints = _max; +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::Plot3D, + ignition::gui::Plugin) diff --git a/src/gui/plugins/plot_3d/Plot3D.hh b/src/gui/plugins/plot_3d/Plot3D.hh new file mode 100644 index 0000000000..103d78149c --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.hh @@ -0,0 +1,142 @@ +/* + * 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_3DPLOT_HH_ +#define IGNITION_GAZEBO_GUI_3DPLOT_HH_ + +#include + +#include + +#include "ignition/gui/qt.h" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ + class Plot3DPrivate; + + /// \brief + class Plot3D : public ignition::gazebo::GuiSystem + { + Q_OBJECT + + /// \brief Target entity + Q_PROPERTY( + Entity targetEntity + READ TargetEntity + WRITE SetTargetEntity + NOTIFY TargetEntityChanged + ) + + /// \brief Target name + Q_PROPERTY( + QString targetName + READ TargetName + WRITE SetTargetName + NOTIFY TargetNameChanged + ) + + /// \brief Locked + Q_PROPERTY( + bool locked + READ Locked + WRITE SetLocked + NOTIFY LockedChanged + ) + + /// \brief Constructor + public: Plot3D(); + + /// \brief Destructor + public: ~Plot3D() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Get the target currently controlled. + /// \return Target entity ID. + public: Q_INVOKABLE Entity TargetEntity() const; + + /// \brief Set the target currently controlled. + /// \param[in] _entity Target entity ID. + public: Q_INVOKABLE void SetTargetEntity(Entity _entity); + + /// \brief Notify that entity has changed. + signals: void TargetEntityChanged(); + + /// \brief Get the name of target currently controlled. + /// \return TargetName, such as 'world' or 'target' + public: Q_INVOKABLE QString TargetName() const; + + /// \brief Set the name of target currently controlled. + /// \param[in] _name TargetName, such as 'world' or 'target'. + public: Q_INVOKABLE void SetTargetName(const QString &_name); + + /// \brief Notify that target name has changed + signals: void TargetNameChanged(); + + /// \brief Get whether the controller is currently locked on a target. + /// \return True for locked + public: Q_INVOKABLE bool Locked() const; + + /// \brief Set whether the controller is currently locked on a target. + /// \param[in] _locked True for locked. + public: Q_INVOKABLE void SetLocked(bool _locked); + + /// \brief Notify that locked has changed. + signals: void LockedChanged(); + + /// \brief Callback to update offset + /// \param[in] _x Offset on X axis + /// \param[in] _y Offset on Y axis + /// \param[in] _z Offset on Z axis + public: Q_INVOKABLE void SetOffset(double _x, double _y, double _z); + + /// \brief Callback to update color + /// \param[in] _r Red + /// \param[in] _g Green + /// \param[in] _b Blue + public: Q_INVOKABLE void SetColor(double _r, double _g, double _b); + + /// \brief Callback to set minimum distance between points. + /// \param[in] _dist Minimum distance + public: Q_INVOKABLE void SetMinDistance(double _dist); + + /// \brief Callback to set maximum number of points + /// \param[in] _max Maximum number of points in the plot. + public: Q_INVOKABLE void SetMaxPoints(int _max); + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} +} + +#endif diff --git a/src/gui/plugins/plot_3d/Plot3D.qml b/src/gui/plugins/plot_3d/Plot3D.qml new file mode 100644 index 0000000000..4672d390ef --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.qml @@ -0,0 +1,308 @@ +/* + * 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.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/qml" + +Rectangle { + id: plot3D + color: "transparent" + Layout.minimumWidth: 400 + Layout.minimumHeight: 375 + anchors.fill: parent + + /** + * Light grey according to theme + */ + property color lightGrey: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade100) : + Material.color(Material.Grey, Material.Shade800) + + /** + * Dark grey according to theme + */ + property color darkGrey: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade200) : + Material.color(Material.Grey, Material.Shade900) + + Connections { + target: Plot3D + onLockedChanged: { + lockButton.checked = Plot3D.Locked() + } + } + + Rectangle { + id: header + height: lockButton.height + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + width: parent.width + color: darkGrey + + RowLayout { + anchors.fill: parent + spacing: 0 + + Label { + text: Plot3D.targetName + color: Material.theme == Material.Light ? "#444444" : "#cccccc" + font.pointSize: 12 + padding: 3 + } + + Item { + height: entityLabel.height + Layout.fillWidth: true + } + + ToolButton { + id: lockButton + checkable: true + checked: false + text: "Lock entity" + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: lockButton.checked ? "qrc:/Gazebo/images/lock.svg" : + "qrc:/Gazebo/images/unlock.svg" + sourceSize.width: 18; + sourceSize.height: 18; + } + ToolTip.text: lockButton.checked ? "Unlock target selection" + : "Lock target selection" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + onToggled: { + Plot3D.locked = lockButton.checked + } + } + + Label { + id: entityLabel + text: 'Entity ' + Plot3D.targetEntity + Layout.minimumWidth: 80 + color: Material.theme == Material.Light ? "#444444" : "#cccccc" + font.pointSize: 12 + padding: 5 + } + } + } + GridLayout { + anchors.top: header.bottom + anchors.bottom: parent.bottom + anchors.left: parent.left + anchors.right: parent.right + columns: 6 + + Text { + text: "Offset" + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 0 + Layout.columnSpan: 6 + color: Material.Grey + leftPadding: 5 + } + + Text { + text: "X (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: x + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 1 + value: 0 + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: Plot3D.SetOffset(x.value, y.value, z.value) + } + Text { + text: "Y (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 2 + leftPadding: 5 + } + IgnSpinBox { + id: y + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 3 + value: 0 + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: Plot3D.SetOffset(x.value, y.value, z.value) + } + Text { + text: "Z (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 4 + leftPadding: 5 + } + IgnSpinBox { + id: z + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 5 + value: 0 + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: Plot3D.SetOffset(x.value, y.value, z.value) + } + + Text { + Layout.columnSpan: 6 + text: "Color" + color: "dimgrey" + font.bold: true + Layout.row: 2 + Layout.column: 0 + } + + Text { + text: "R" + color: "dimgrey" + Layout.row: 3 + Layout.column: 0 + leftPadding: 5 + } + + IgnSpinBox { + id: r + Layout.row: 3 + Layout.column: 1 + maximumValue: 1.00 + minimumValue: 0.00 + value: 0 + stepSize: 0.01 + decimals: 2 + onEditingFinished: Plot3D.SetColor(r.value, g.value, b.value) + } + + Text { + text: "G" + Layout.row: 3 + Layout.column: 2 + color: "dimgrey" + leftPadding: 5 + } + + IgnSpinBox { + id: g + Layout.row: 3 + Layout.column: 3 + maximumValue: 1.00 + minimumValue: 0.00 + value: 0 + stepSize: 0.01 + decimals: 2 + onEditingFinished: Plot3D.SetColor(r.value, g.value, b.value) + } + + Text { + text: "B" + Layout.row: 3 + Layout.column: 4 + color: "dimgrey" + leftPadding: 5 + } + + IgnSpinBox { + id: b + Layout.row: 3 + Layout.column: 5 + maximumValue: 1.00 + minimumValue: 0.00 + value: 1.0 + stepSize: 0.01 + decimals: 2 + onEditingFinished: Plot3D.SetColor(r.value, g.value, b.value) + } + + Text { + text: "Min distance between points (m)" + color: "dimgrey" + Layout.row: 4 + Layout.column: 0 + Layout.columnSpan: 3 + leftPadding: 5 + } + IgnSpinBox { + id: minDist + Layout.fillWidth: true + Layout.row: 4 + Layout.column: 3 + Layout.columnSpan: 3 + value: 0.05 + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: Plot3D.SetMinDistance(minDist.value) + } + + Text { + text: "Max points" + color: "dimgrey" + Layout.row: 5 + Layout.column: 0 + Layout.columnSpan: 3 + leftPadding: 5 + } + IgnSpinBox { + id: maxPoints + Layout.fillWidth: true + Layout.row: 5 + Layout.column: 3 + Layout.columnSpan: 3 + value: 1000 + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 0 + stepSize: 100 + onEditingFinished: Plot3D.SetMaxPoints(maxPoints.value) + } + + // Bottom spacer + Item { + width: 10 + Layout.row: 6 + Layout.column: 0 + Layout.columnSpan: 6 + Layout.fillHeight: true + } + } +} diff --git a/src/gui/plugins/plot_3d/Plot3D.qrc b/src/gui/plugins/plot_3d/Plot3D.qrc new file mode 100644 index 0000000000..f1327f014d --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.qrc @@ -0,0 +1,5 @@ + + + Plot3D.qml + + From 146f9d4b53feb34ad27c230af22dcaebf482635e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 15 Jul 2021 12:52:41 -0700 Subject: [PATCH 02/10] add example Signed-off-by: Louise Poubel --- examples/worlds/plot_3d.sdf | 155 ++++++++++++++++++++++++++++++++++++ 1 file changed, 155 insertions(+) create mode 100644 examples/worlds/plot_3d.sdf diff --git a/examples/worlds/plot_3d.sdf b/examples/worlds/plot_3d.sdf new file mode 100644 index 0000000000..4dd80ea80f --- /dev/null +++ b/examples/worlds/plot_3d.sdf @@ -0,0 +1,155 @@ + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + + + + + Upper link plot + + Double pendulum::upper_link + 0 0 1 + 0 0 1 + 20 + 0.1 + + + + + + Lower link plot + + Double pendulum::lower_link + 0 1 0 + 0 0 1 + 40 + 0.3 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + Double pendulum + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base + + + + From a7bc590688fdc3a18890c573f01e227eb9377350 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 15 Jul 2021 16:52:36 -0700 Subject: [PATCH 03/10] improve UX Signed-off-by: Louise Poubel --- examples/worlds/plot_3d.sdf | 2 +- src/gui/plugins/plot_3d/CMakeLists.txt | 2 + src/gui/plugins/plot_3d/Plot3D.cc | 104 ++++++++++++++++------- src/gui/plugins/plot_3d/Plot3D.hh | 100 +++++++++++++++++----- src/gui/plugins/plot_3d/Plot3D.qml | 60 +++++++------ src/gui/plugins/plot_3d/Plot3D_TEST.cc | 112 +++++++++++++++++++++++++ src/gui/plugins/scene3d/Scene3D.cc | 3 - 7 files changed, 302 insertions(+), 81 deletions(-) create mode 100644 src/gui/plugins/plot_3d/Plot3D_TEST.cc diff --git a/examples/worlds/plot_3d.sdf b/examples/worlds/plot_3d.sdf index 4dd80ea80f..2ee6b428e7 100644 --- a/examples/worlds/plot_3d.sdf +++ b/examples/worlds/plot_3d.sdf @@ -25,7 +25,7 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - -6 0 6 0 0.5 0 + 5 -1.5 3 0 0.37 2.8 diff --git a/src/gui/plugins/plot_3d/CMakeLists.txt b/src/gui/plugins/plot_3d/CMakeLists.txt index 73423d7090..3ff591a861 100644 --- a/src/gui/plugins/plot_3d/CMakeLists.txt +++ b/src/gui/plugins/plot_3d/CMakeLists.txt @@ -3,4 +3,6 @@ gz_add_gui_plugin(Plot3D Plot3D.cc QT_HEADERS Plot3D.hh + TEST_SOURCES + Plot3D_TEST.cc ) diff --git a/src/gui/plugins/plot_3d/Plot3D.cc b/src/gui/plugins/plot_3d/Plot3D.cc index 7581bb87d9..e194e6cb8d 100644 --- a/src/gui/plugins/plot_3d/Plot3D.cc +++ b/src/gui/plugins/plot_3d/Plot3D.cc @@ -39,11 +39,7 @@ #include "Plot3D.hh" -namespace ignition -{ -namespace gazebo -{ -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +namespace ignition::gazebo::gui { /// \brief Private data class for Plot3D class Plot3DPrivate @@ -75,9 +71,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Previous plotted position. public: math::Vector3d prevPos; - /// \brief Offset from entity origin to place plot point. The orientation - /// isn't used. The offset is expressed in the entity's frame. - public: math::Pose3d offset; + /// \brief Offset from entity origin to place plot point. + /// The offset is expressed in the entity's frame. + public: math::Vector3d offset; /// \brief Minimum distance between points. If the object has moved by less /// than this distance, a new point isn't plotted. @@ -91,11 +87,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE public: std::mutex mutex; }; } -} -} using namespace ignition; -using namespace gazebo; +using namespace ignition::gazebo; +using namespace ignition::gazebo::gui; ///////////////////////////////////////////////// Plot3D::Plot3D() @@ -105,7 +100,10 @@ Plot3D::Plot3D() } ///////////////////////////////////////////////// -Plot3D::~Plot3D() = default; +Plot3D::~Plot3D() +{ + this->ClearPlot(); +} ///////////////////////////////////////////////// void Plot3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) @@ -121,17 +119,16 @@ void Plot3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { this->dataPtr->targetName = nameElem->GetText(); this->dataPtr->targetNameDirty = true; - this->dataPtr->locked = true; + this->SetLocked(true); } auto offsetElem = _pluginElem->FirstChildElement("offset"); if (nullptr != offsetElem && nullptr != offsetElem->GetText()) { - math::Vector3d pos; std::stringstream offsetStr; offsetStr << std::string(offsetElem->GetText()); - offsetStr >> pos; - this->dataPtr->offset.Set(pos, math::Vector3d::Zero); + offsetStr >> this->dataPtr->offset; + this->OffsetChanged(); } auto colorElem = _pluginElem->FirstChildElement("color"); @@ -140,18 +137,21 @@ void Plot3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) std::stringstream colorStr; colorStr << std::string(colorElem->GetText()); colorStr >> this->dataPtr->color; + this->ColorChanged(); } auto distElem = _pluginElem->FirstChildElement("minimum_distance"); if (nullptr != distElem && nullptr != distElem->GetText()) { distElem->QueryDoubleText(&this->dataPtr->minDistance); + this->MinDistanceChanged(); } auto ptsElem = _pluginElem->FirstChildElement("maximum_points"); if (nullptr != ptsElem && nullptr != ptsElem->GetText()) { ptsElem->QueryInt64Text(&this->dataPtr->maxPoints); + this->MaxPointsChanged(); } } @@ -159,6 +159,17 @@ void Plot3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignition::gui::MainWindow *>()->installEventFilter(this); } +///////////////////////////////////////////////// +void Plot3D::ClearPlot() +{ + // Clear previous plot + if (this->dataPtr->markerMsg.point().size() > 0) + { + this->dataPtr->markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER); + this->dataPtr->node.Request("/marker", this->dataPtr->markerMsg); + } +} + ////////////////////////////////////////////////// void Plot3D::Update(const UpdateInfo &, EntityComponentManager &_ecm) { @@ -199,9 +210,7 @@ void Plot3D::Update(const UpdateInfo &, EntityComponentManager &_ecm) if (newTarget) { - // Clear previous plot - this->dataPtr->markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER); - this->dataPtr->node.Request("/marker", this->dataPtr->markerMsg); + this->ClearPlot(); // Reset message this->dataPtr->markerMsg.Clear(); @@ -218,8 +227,10 @@ void Plot3D::Update(const UpdateInfo &, EntityComponentManager &_ecm) // Get entity pose auto pose = worldPose(this->dataPtr->targetEntity, _ecm); + math::Pose3d offsetPose; + offsetPose.Set(this->dataPtr->offset, math::Vector3d::Zero); - auto point = (pose * this->dataPtr->offset).Pos(); + auto point = (pose * offsetPose).Pos(); // Only add points if the distance is past a threshold. if (point.Distance(this->dataPtr->prevPos) < this->dataPtr->minDistance) @@ -238,7 +249,6 @@ void Plot3D::Update(const UpdateInfo &, EntityComponentManager &_ecm) this->dataPtr->color); // Request -//igndbg << this->dataPtr->markerMsg.DebugString() << std::endl; this->dataPtr->node.Request("/marker", this->dataPtr->markerMsg); } @@ -318,29 +328,63 @@ void Plot3D::SetLocked(bool _locked) } ///////////////////////////////////////////////// -void Plot3D::SetOffset(double _x, double _y, double _z) +QVector3D Plot3D::Offset() const +{ + return QVector3D( + this->dataPtr->offset.X(), + this->dataPtr->offset.Y(), + this->dataPtr->offset.Z()); +} + +///////////////////////////////////////////////// +void Plot3D::SetOffset(const QVector3D &_offset) +{ + this->dataPtr->offset.Set(_offset.x(), _offset.y(), _offset.z()); + this->OffsetChanged(); +} + +///////////////////////////////////////////////// +QVector3D Plot3D::Color() const +{ + return QVector3D( + this->dataPtr->color.R(), + this->dataPtr->color.G(), + this->dataPtr->color.B()); +} + +///////////////////////////////////////////////// +void Plot3D::SetColor(const QVector3D &_color) +{ + this->dataPtr->color.Set(_color.x(), _color.y(), _color.z()); + this->ColorChanged(); +} + +///////////////////////////////////////////////// +double Plot3D::MinDistance() const { - this->dataPtr->offset.Set(_x, _y, _z, 0, 0, 0); + return this->dataPtr->minDistance; } ///////////////////////////////////////////////// -void Plot3D::SetColor(double _r, double _g, double _b) +void Plot3D::SetMinDistance(double _minDistance) { - this->dataPtr->color.Set(_r, _g, _b); + this->dataPtr->minDistance = _minDistance; + this->MinDistanceChanged(); } ///////////////////////////////////////////////// -void Plot3D::SetMinDistance(double _dist) +int Plot3D::MaxPoints() const { - this->dataPtr->minDistance = _dist; + return this->dataPtr->maxPoints; } ///////////////////////////////////////////////// -void Plot3D::SetMaxPoints(int _max) +void Plot3D::SetMaxPoints(int _maxPoints) { - this->dataPtr->maxPoints = _max; + this->dataPtr->maxPoints = _maxPoints; + this->MaxPointsChanged(); } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::Plot3D, +IGNITION_ADD_PLUGIN(ignition::gazebo::gui::Plot3D, ignition::gui::Plugin) diff --git a/src/gui/plugins/plot_3d/Plot3D.hh b/src/gui/plugins/plot_3d/Plot3D.hh index 103d78149c..96910d040a 100644 --- a/src/gui/plugins/plot_3d/Plot3D.hh +++ b/src/gui/plugins/plot_3d/Plot3D.hh @@ -28,8 +28,7 @@ namespace ignition { namespace gazebo { -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +namespace gui { class Plot3DPrivate; @@ -62,6 +61,38 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE NOTIFY LockedChanged ) + /// \brief Locked + Q_PROPERTY( + QVector3D offset + READ Offset + WRITE SetOffset + NOTIFY OffsetChanged + ) + + /// \brief Locked + Q_PROPERTY( + QVector3D color + READ Color + WRITE SetColor + NOTIFY ColorChanged + ) + + /// \brief Minimum distance + Q_PROPERTY( + double minDistance + READ MinDistance + WRITE SetMinDistance + NOTIFY MinDistanceChanged + ) + + /// \brief Maximum points + Q_PROPERTY( + int maxPoints + READ MaxPoints + WRITE SetMaxPoints + NOTIFY MaxPointsChanged + ) + /// \brief Constructor public: Plot3D(); @@ -97,40 +128,69 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Notify that target name has changed signals: void TargetNameChanged(); - /// \brief Get whether the controller is currently locked on a target. + /// \brief Get whether the plugin is currently locked on a target. /// \return True for locked public: Q_INVOKABLE bool Locked() const; - /// \brief Set whether the controller is currently locked on a target. + /// \brief Set whether the plugin is currently locked on a target. /// \param[in] _locked True for locked. public: Q_INVOKABLE void SetLocked(bool _locked); /// \brief Notify that locked has changed. signals: void LockedChanged(); - /// \brief Callback to update offset - /// \param[in] _x Offset on X axis - /// \param[in] _y Offset on Y axis - /// \param[in] _z Offset on Z axis - public: Q_INVOKABLE void SetOffset(double _x, double _y, double _z); + /// \brief Get the offset in the target's frame. + /// \return The current offset. + public: Q_INVOKABLE QVector3D Offset() const; + + /// \brief Set the offset. + /// \param[in] _offset The offset in the target's frame + public: Q_INVOKABLE void SetOffset(const QVector3D &_offset); + + /// \brief Notify that offset has changed. + signals: void OffsetChanged(); + + /// \brief Get the color of the plot. + /// \return The current color. + public: Q_INVOKABLE QVector3D Color() const; + + /// \brief Set the color of the plot. + /// \param[in] _color New color. + public: Q_INVOKABLE void SetColor(const QVector3D &_color); - /// \brief Callback to update color - /// \param[in] _r Red - /// \param[in] _g Green - /// \param[in] _b Blue - public: Q_INVOKABLE void SetColor(double _r, double _g, double _b); + /// \brief Notify that offset has changed. + signals: void ColorChanged(); - /// \brief Callback to set minimum distance between points. - /// \param[in] _dist Minimum distance - public: Q_INVOKABLE void SetMinDistance(double _dist); + /// \brief Get the minimum distance between points. + /// \return The current minimum distance. + public: Q_INVOKABLE double MinDistance() const; - /// \brief Callback to set maximum number of points - /// \param[in] _max Maximum number of points in the plot. - public: Q_INVOKABLE void SetMaxPoints(int _max); + /// \brief Set the minimum distance between points. If the target moved + /// less than this distance, the latest position won't be plotted. + /// \param[in] _minDistance New distance. + public: Q_INVOKABLE void SetMinDistance(double _minDistance); + + /// \brief Notify that the minimum distance has changed. + signals: void MinDistanceChanged(); + + /// \brief Get the maximum points between points. + /// \return The current maximum points. + public: Q_INVOKABLE int MaxPoints() const; + + /// \brief Set the maximum points between points. If the target moved + /// less than this distance, the latest position won't be plotted. + /// \param[in] _maxPoints New distance. + public: Q_INVOKABLE void SetMaxPoints(int _maxPoints); + + /// \brief Notify that the maximum points has changed. + signals: void MaxPointsChanged(); // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + /// \brief Clear plot + private: void ClearPlot(); + /// \internal /// \brief Pointer to private data private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/plot_3d/Plot3D.qml b/src/gui/plugins/plot_3d/Plot3D.qml index 4672d390ef..3ba4a26f61 100644 --- a/src/gui/plugins/plot_3d/Plot3D.qml +++ b/src/gui/plugins/plot_3d/Plot3D.qml @@ -26,7 +26,7 @@ Rectangle { id: plot3D color: "transparent" Layout.minimumWidth: 400 - Layout.minimumHeight: 375 + Layout.minimumHeight: 320 anchors.fill: parent /** @@ -121,7 +121,8 @@ Rectangle { Layout.row: 0 Layout.column: 0 Layout.columnSpan: 6 - color: Material.Grey + color: "dimgrey" + font.bold: true leftPadding: 5 } @@ -137,12 +138,12 @@ Rectangle { Layout.fillWidth: true Layout.row: 1 Layout.column: 1 - value: 0 + value: Plot3D.offset.x maximumValue: 1000000 minimumValue: -1000000 - decimals: 6 + decimals: 2 stepSize: 0.01 - onEditingFinished: Plot3D.SetOffset(x.value, y.value, z.value) + onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) } Text { text: "Y (m)" @@ -156,12 +157,12 @@ Rectangle { Layout.fillWidth: true Layout.row: 1 Layout.column: 3 - value: 0 + value: Plot3D.offset.y maximumValue: 1000000 minimumValue: -1000000 - decimals: 6 + decimals: 2 stepSize: 0.01 - onEditingFinished: Plot3D.SetOffset(x.value, y.value, z.value) + onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) } Text { text: "Z (m)" @@ -175,21 +176,23 @@ Rectangle { Layout.fillWidth: true Layout.row: 1 Layout.column: 5 - value: 0 + value: Plot3D.offset.z maximumValue: 1000000 minimumValue: -1000000 - decimals: 6 + decimals: 2 stepSize: 0.01 - onEditingFinished: Plot3D.SetOffset(x.value, y.value, z.value) + onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) } Text { - Layout.columnSpan: 6 text: "Color" - color: "dimgrey" - font.bold: true + Layout.fillWidth: true Layout.row: 2 Layout.column: 0 + Layout.columnSpan: 6 + color: "dimgrey" + font.bold: true + leftPadding: 5 } Text { @@ -202,14 +205,15 @@ Rectangle { IgnSpinBox { id: r + Layout.fillWidth: true Layout.row: 3 Layout.column: 1 + value: Plot3D.color.x maximumValue: 1.00 minimumValue: 0.00 - value: 0 - stepSize: 0.01 decimals: 2 - onEditingFinished: Plot3D.SetColor(r.value, g.value, b.value) + stepSize: 0.01 + onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) } Text { @@ -222,14 +226,15 @@ Rectangle { IgnSpinBox { id: g + Layout.fillWidth: true Layout.row: 3 Layout.column: 3 + value: Plot3D.color.y maximumValue: 1.00 minimumValue: 0.00 - value: 0 - stepSize: 0.01 decimals: 2 - onEditingFinished: Plot3D.SetColor(r.value, g.value, b.value) + stepSize: 0.01 + onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) } Text { @@ -242,14 +247,15 @@ Rectangle { IgnSpinBox { id: b + Layout.fillWidth: true Layout.row: 3 Layout.column: 5 + value: Plot3D.color.z maximumValue: 1.00 minimumValue: 0.00 - value: 1.0 - stepSize: 0.01 decimals: 2 - onEditingFinished: Plot3D.SetColor(r.value, g.value, b.value) + stepSize: 0.01 + onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) } Text { @@ -266,9 +272,9 @@ Rectangle { Layout.row: 4 Layout.column: 3 Layout.columnSpan: 3 - value: 0.05 + value: Plot3D.minDistance maximumValue: 1000000 - minimumValue: -1000000 + minimumValue: 0 decimals: 6 stepSize: 0.01 onEditingFinished: Plot3D.SetMinDistance(minDist.value) @@ -288,9 +294,9 @@ Rectangle { Layout.row: 5 Layout.column: 3 Layout.columnSpan: 3 - value: 1000 + value: Plot3D.maxPoints maximumValue: 1000000 - minimumValue: -1000000 + minimumValue: 0 decimals: 0 stepSize: 100 onEditingFinished: Plot3D.SetMaxPoints(maxPoints.value) diff --git a/src/gui/plugins/plot_3d/Plot3D_TEST.cc b/src/gui/plugins/plot_3d/Plot3D_TEST.cc new file mode 100644 index 0000000000..36ca1691fb --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -0,0 +1,112 @@ +/* + * 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 +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointType.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../../GuiRunner.hh" +#include "Plot3D.hh" + +int g_argc = 1; +char **g_argv; + +using namespace ignition; + +/// \brief Tests for the joint position controller GUI plugin +class Plot3D : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(Plot3D, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) +{ + // Create app + auto app = std::make_unique(g_argc, g_argv); + ASSERT_NE(nullptr, app); + app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + // Create GUI runner to handle gazebo::gui plugins + auto runner = new gazebo::GuiRunner("test"); + runner->connect(app.get(), &gui::Application::PluginAdded, + runner, &gazebo::GuiRunner::OnPluginAdded); + runner->setParent(gui::App()); + + // Add plugin + const char *pluginStr = + "" + "" + "Plot3D!" + "" + "banana" + "123" + "0.123" + "1 2 3" + "0.1 0.2 0.3" + ""; + + tinyxml2::XMLDocument pluginDoc; + EXPECT_EQ(tinyxml2::XML_SUCCESS, pluginDoc.Parse(pluginStr)); + EXPECT_TRUE(app->LoadPlugin("Plot3D", + pluginDoc.FirstChildElement("plugin"))); + + // Get main window + auto win = app->findChild(); + ASSERT_NE(nullptr, win); + + // Get plugin + auto plugins = win->findChildren(); + ASSERT_EQ(plugins.size(), 1); + + auto plugin = plugins[0]; + EXPECT_EQ(plugin->Title(), "Plot3D!"); + EXPECT_EQ(plugin->TargetEntity(), gazebo::kNullEntity); + EXPECT_EQ(plugin->TargetName(), QString("banana")) + << plugin->TargetName().toStdString(); + EXPECT_EQ(plugin->Color(), QVector3D(0.1, 0.2, 0.3)); + EXPECT_EQ(plugin->Offset(), QVector3D(1, 2, 3)); + EXPECT_TRUE(plugin->Locked()); + + // Cleanup + plugins.clear(); +} diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 66c31073c3..9de8888404 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -1372,9 +1372,6 @@ void IgnRenderer::HandleMouseTransformControl() // Select entity else if (!this->dataPtr->mouseEvent.Dragging()) { - rendering::VisualPtr v = this->dataPtr->camera->VisualAt( - this->dataPtr->mouseEvent.Pos()); - rendering::VisualPtr visual = this->dataPtr->camera->Scene()->VisualAt( this->dataPtr->camera, this->dataPtr->mouseEvent.Pos()); From 4b0823a359767b86f4ea67114deb1eb63a7b7596 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 15 Jul 2021 17:03:28 -0700 Subject: [PATCH 04/10] docs Signed-off-by: Louise Poubel --- src/gui/plugins/plot_3d/Plot3D.hh | 17 ++++++++++++++++- src/gui/plugins/plot_3d/Plot3D.qml | 2 +- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/gui/plugins/plot_3d/Plot3D.hh b/src/gui/plugins/plot_3d/Plot3D.hh index 96910d040a..477a1405df 100644 --- a/src/gui/plugins/plot_3d/Plot3D.hh +++ b/src/gui/plugins/plot_3d/Plot3D.hh @@ -32,7 +32,22 @@ namespace gui { class Plot3DPrivate; - /// \brief + /// \brief Plot the trajectory of an entity into the 3D scene. + /// + /// This plugin can be instantiated multiple times to plot various entities. + /// + /// ## Configuration + /// + /// ``: Plot the given entity at startup. Accepts names scoped + /// with `::`, for example `my_model::my_link` + /// ``: RGB color of line, defaults to blue. + /// ``: XYZ offset from the entity's origin to plot from, expressed + /// in the entity's frame. Defaults to zero. + /// ``: The minimum distance between points to plot. A new + /// point will not be plotted until the entity has moved beyond this distance + /// from the previous point. + /// ``: Maximum number of points on the plot. After this + /// number is reached, the older points start being deleted. class Plot3D : public ignition::gazebo::GuiSystem { Q_OBJECT diff --git a/src/gui/plugins/plot_3d/Plot3D.qml b/src/gui/plugins/plot_3d/Plot3D.qml index 3ba4a26f61..acbbf1708c 100644 --- a/src/gui/plugins/plot_3d/Plot3D.qml +++ b/src/gui/plugins/plot_3d/Plot3D.qml @@ -64,7 +64,7 @@ Rectangle { spacing: 0 Label { - text: Plot3D.targetName + text: Plot3D.targetName.empty ? "No entity selected" : Plot3D.targetName color: Material.theme == Material.Light ? "#444444" : "#cccccc" font.pointSize: 12 padding: 3 From 071acf9daf0682bfb3452653cd7e1a18ff13f7e6 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 16 Jul 2021 14:43:14 -0700 Subject: [PATCH 05/10] PR feedback Signed-off-by: Louise Poubel --- examples/worlds/plot_3d.sdf | 3 --- include/ignition/gazebo/Util.hh | 16 +++++++++++++++- src/gui/plugins/plot_3d/Plot3D_TEST.cc | 5 ++++- 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/examples/worlds/plot_3d.sdf b/examples/worlds/plot_3d.sdf index 2ee6b428e7..fe482cddfe 100644 --- a/examples/worlds/plot_3d.sdf +++ b/examples/worlds/plot_3d.sdf @@ -48,7 +48,6 @@ true true true - @@ -72,7 +71,6 @@ true true true - @@ -102,7 +100,6 @@ 40 0.3 - diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 635d744bdf..d18135cb37 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -52,11 +52,25 @@ namespace ignition bool _includePrefix = true); /// \brief Helper function to get an entity given its scoped name. + /// The scope may start at any level. For example, in this hierarchy: + /// + /// world_name + /// model_name + /// link_name + /// + /// All these names will return the link entity: + /// + /// * world_name::model_name::link_name + /// * model_name::link_name + /// * link_name + /// /// \param[in] _scopedName Entity's scoped name. /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _delim Delimiter between names, defaults to "::". + /// \return The engity or kNullEntity if no entity was found with that + /// name. Entity IGNITION_GAZEBO_VISIBLE entityFromScopedName( - const std::string &_entity, const EntityComponentManager &_ecm, + const std::string &_scopedName, const EntityComponentManager &_ecm, const std::string &_delim = "::"); /// \brief Generally, each entity will be of some specific high-level type, diff --git a/src/gui/plugins/plot_3d/Plot3D_TEST.cc b/src/gui/plugins/plot_3d/Plot3D_TEST.cc index 36ca1691fb..1ea29be3b6 100644 --- a/src/gui/plugins/plot_3d/Plot3D_TEST.cc +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -44,7 +44,10 @@ #include "Plot3D.hh" int g_argc = 1; -char **g_argv; +char* g_argv[] = +{ + reinterpret_cast(const_cast("dummy")), +}; using namespace ignition; From 2bd7b67182d25493606e2f4254b67273c1a638b1 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 19 Jul 2021 19:02:03 -0700 Subject: [PATCH 06/10] docs Signed-off-by: Louise Poubel --- src/gui/plugins/plot_3d/Plot3D.hh | 55 ++++++++++++++++++------------- 1 file changed, 33 insertions(+), 22 deletions(-) diff --git a/src/gui/plugins/plot_3d/Plot3D.hh b/src/gui/plugins/plot_3d/Plot3D.hh index 477a1405df..a46b90a1f0 100644 --- a/src/gui/plugins/plot_3d/Plot3D.hh +++ b/src/gui/plugins/plot_3d/Plot3D.hh @@ -36,18 +36,29 @@ namespace gui /// /// This plugin can be instantiated multiple times to plot various entities. /// + /// The plugin is automatically attached to the currently selected entity, + /// unless it is locked on an entity. + /// /// ## Configuration /// - /// ``: Plot the given entity at startup. Accepts names scoped - /// with `::`, for example `my_model::my_link` - /// ``: RGB color of line, defaults to blue. - /// ``: XYZ offset from the entity's origin to plot from, expressed - /// in the entity's frame. Defaults to zero. - /// ``: The minimum distance between points to plot. A new - /// point will not be plotted until the entity has moved beyond this distance - /// from the previous point. - /// ``: Maximum number of points on the plot. After this - /// number is reached, the older points start being deleted. + /// * `` (optional): Plot the given entity at startup. Accepts + /// names scoped with `::`, for example `my_model::my_link`. If not provided, + /// the plugin starts not attached to any entity, and will attach to the + /// next selected entity. + /// + /// * `` (optional): RGB color of line, defaults to blue. + /// + /// * `` (optional): XYZ offset from the entity's origin to plot from, + /// expressed in the entity's frame. Defaults to zero. + /// + /// * `` (optional): The minimum distance between points to + /// plot. A new point will not be plotted until the entity has moved beyond + /// this distance from the previous point. Defaults to 5 cm. + /// + /// * ` (optional)`: Maximum number of points on the plot. + /// After this number is reached, the older points start being deleted. + /// Defaults to 1000. + /// class Plot3D : public ignition::gazebo::GuiSystem { Q_OBJECT @@ -60,7 +71,7 @@ namespace gui NOTIFY TargetEntityChanged ) - /// \brief Target name + /// \brief Target entity scoped name Q_PROPERTY( QString targetName READ TargetName @@ -68,7 +79,7 @@ namespace gui NOTIFY TargetNameChanged ) - /// \brief Locked + /// \brief Whether the plugin is locked on an entity Q_PROPERTY( bool locked READ Locked @@ -76,7 +87,7 @@ namespace gui NOTIFY LockedChanged ) - /// \brief Locked + /// \brief XYZ offset to the entity's origin to plot Q_PROPERTY( QVector3D offset READ Offset @@ -84,7 +95,7 @@ namespace gui NOTIFY OffsetChanged ) - /// \brief Locked + /// \brief Plot line color Q_PROPERTY( QVector3D color READ Color @@ -92,7 +103,7 @@ namespace gui NOTIFY ColorChanged ) - /// \brief Minimum distance + /// \brief Minimum distance between plot points Q_PROPERTY( double minDistance READ MinDistance @@ -100,7 +111,7 @@ namespace gui NOTIFY MinDistanceChanged ) - /// \brief Maximum points + /// \brief Maximum number of total points on the plot Q_PROPERTY( int maxPoints READ MaxPoints @@ -162,7 +173,7 @@ namespace gui /// \param[in] _offset The offset in the target's frame public: Q_INVOKABLE void SetOffset(const QVector3D &_offset); - /// \brief Notify that offset has changed. + /// \brief Notify that the offset has changed. signals: void OffsetChanged(); /// \brief Get the color of the plot. @@ -173,7 +184,7 @@ namespace gui /// \param[in] _color New color. public: Q_INVOKABLE void SetColor(const QVector3D &_color); - /// \brief Notify that offset has changed. + /// \brief Notify that the color has changed. signals: void ColorChanged(); /// \brief Get the minimum distance between points. @@ -188,13 +199,13 @@ namespace gui /// \brief Notify that the minimum distance has changed. signals: void MinDistanceChanged(); - /// \brief Get the maximum points between points. + /// \brief Get the maximum number of points. /// \return The current maximum points. public: Q_INVOKABLE int MaxPoints() const; - /// \brief Set the maximum points between points. If the target moved - /// less than this distance, the latest position won't be plotted. - /// \param[in] _maxPoints New distance. + /// \brief Set the maximum number of points. If the plot has more than + /// this number, older points start being removed. + /// \param[in] _maxPoints Maximum number of points. public: Q_INVOKABLE void SetMaxPoints(int _maxPoints); /// \brief Notify that the maximum points has changed. From e6b843b0add779b90c18f474533b94b6803fd299 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 23 Jul 2021 21:47:21 -0700 Subject: [PATCH 07/10] PR feedback Signed-off-by: Louise Poubel --- include/ignition/gazebo/Util.hh | 4 +- src/Util.cc | 41 +-- src/Util_TEST.cc | 125 ++++++--- src/gui/plugins/plot_3d/Plot3D.cc | 21 +- src/gui/plugins/plot_3d/Plot3D.hh | 2 +- src/gui/plugins/plot_3d/Plot3D.qml | 351 +++++++++++++------------ src/gui/plugins/plot_3d/Plot3D_TEST.cc | 10 +- 7 files changed, 322 insertions(+), 232 deletions(-) diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index d18135cb37..f9e72b110e 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -67,9 +67,9 @@ namespace ignition /// \param[in] _scopedName Entity's scoped name. /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _delim Delimiter between names, defaults to "::". - /// \return The engity or kNullEntity if no entity was found with that + /// \return The entity or kNullEntity if no entity was found with that /// name. - Entity IGNITION_GAZEBO_VISIBLE entityFromScopedName( + std::unordered_set IGNITION_GAZEBO_VISIBLE entitiesFromScopedName( const std::string &_scopedName, const EntityComponentManager &_ecm, const std::string &_delim = "::"); diff --git a/src/Util.cc b/src/Util.cc index 29102f0cbc..1401673f3c 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -61,7 +61,11 @@ math::Pose3d worldPose(const Entity &_entity, { auto poseComp = _ecm.Component(_entity); if (nullptr == poseComp) + { + ignwarn << "Trying to get world pose from entity [" << _entity + << "], which doesn't have a pose component" << std::endl; return math::Pose3d(); + } // work out pose in world frame math::Pose3d pose = poseComp->Data(); @@ -130,11 +134,11 @@ std::string scopedName(const Entity &_entity, } ////////////////////////////////////////////////// -Entity entityFromScopedName(const std::string &_scopedName, - const EntityComponentManager &_ecm, const std::string &_delim) +std::unordered_set entitiesFromScopedName( + const std::string &_scopedName, const EntityComponentManager &_ecm, + const std::string &_delim) { - Entity result{kNullEntity}; - + // Split names std::vector names; size_t pos1 = 0; size_t pos2 = _scopedName.find(_delim); @@ -142,31 +146,34 @@ Entity entityFromScopedName(const std::string &_scopedName, { names.push_back(_scopedName.substr(pos1, pos2 - pos1)); pos1 = pos2 + _delim.length(); - pos2 = _scopedName.find(_delim, pos2 + _delim.length()); + pos2 = _scopedName.find(_delim, pos1); } names.push_back(_scopedName.substr(pos1, _scopedName.size()-pos1)); + std::vector resVector; for (const auto &name : names) { - Entity entity; - if (kNullEntity == result) + std::vector current; + if (resVector.empty()) { - entity = _ecm.EntityByComponents(components::Name(name)); + current = _ecm.EntitiesByComponents(components::Name(name)); } else { - entity = _ecm.EntityByComponents(components::Name(name), - components::ParentEntity(result)); - } - - if (kNullEntity == entity) - { - return kNullEntity; + for (auto res : resVector) + { + auto matches = _ecm.EntitiesByComponents(components::Name(name), + components::ParentEntity(res)); + std::copy(std::begin(matches), std::end(matches), + std::back_inserter(current)); + } } - result = entity; + if (current.empty()) + return {}; + resVector = current; } - return result; + return std::unordered_set(resVector.begin(), resVector.end()); } ////////////////////////////////////////////////// diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 1bbdfaef18..fa2bba9a4b 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -216,37 +216,102 @@ TEST(UtilTest, ScopedName) EXPECT_EQ(worldEntity, gazebo::worldEntity(linkCCEntity, ecm)); EXPECT_EQ(worldEntity, gazebo::worldEntity(actorDEntity, ecm)); EXPECT_EQ(kNullEntity, gazebo::worldEntity(kNullEntity, ecm)); +} + +///////////////////////////////////////////////// +TEST_F(UtilTest, EntitiesFromScopedName) +{ + EntityComponentManager ecm; - // Entity from scoped name - EXPECT_EQ(kNullEntity, gazebo::entityFromScopedName("banana", ecm)); - EXPECT_EQ(kNullEntity, gazebo::entityFromScopedName( - "world_name::banana", ecm)); - EXPECT_EQ(kNullEntity, gazebo::entityFromScopedName( - "modelB_name::banana", ecm)); - - EXPECT_EQ(worldEntity, gazebo::entityFromScopedName( - "world_name", ecm)); - - EXPECT_EQ(modelBEntity, gazebo::entityFromScopedName( - "world_name::modelB_name", ecm)); - EXPECT_EQ(modelBEntity, gazebo::entityFromScopedName( - "modelB_name", ecm)); - - EXPECT_EQ(linkBEntity, gazebo::entityFromScopedName( - "world_name::modelB_name::linkB_name", ecm)); - EXPECT_EQ(linkBEntity, gazebo::entityFromScopedName( - "modelB_name::linkB_name", ecm)); - EXPECT_EQ(linkBEntity, gazebo::entityFromScopedName( - "linkB_name", ecm)); - - EXPECT_EQ(sensorBEntity, gazebo::entityFromScopedName( - "world_name::modelB_name::linkB_name::sensorB_name", ecm)); - EXPECT_EQ(sensorBEntity, gazebo::entityFromScopedName( - "modelB_name::linkB_name::sensorB_name", ecm)); - EXPECT_EQ(sensorBEntity, gazebo::entityFromScopedName( - "linkB_name::sensorB_name", ecm)); - EXPECT_EQ(sensorBEntity, gazebo::entityFromScopedName( - "sensorB_name", ecm)); + // banana 1 + // - orange 2 + // - plum 3 + // - grape 4 + // - pear 5 + // - plum 6 + // - grape 7 + // - pear 8 + // - plum 9 + // - pear 10 + // - grape 11 + // - pear 12 + // - orange 13 + // - orange 14 + + auto createEntity = [&ecm](const std::string &_name, Entity _parent) -> Entity + { + auto res = ecm.CreateEntity(); + ecm.CreateComponent(res, components::Name(_name)); + ecm.CreateComponent(res, components::ParentEntity(_parent)); + return res; + }; + + auto banana1 = ecm.CreateEntity(); + ecm.CreateComponent(banana1, components::Name("banana")); + + auto orange2 = createEntity("orange", banana1); + auto plum3 = createEntity("plum", orange2); + auto grape4 = createEntity("grape", plum3); + auto pear5 = createEntity("pear", grape4); + auto plum6 = createEntity("plum", pear5); + auto grape7 = createEntity("grape", banana1); + auto pear8 = createEntity("pear", grape7); + auto plum9 = createEntity("plum", pear8); + auto pear10 = createEntity("pear", plum9); + auto grape11 = createEntity("grape", banana1); + auto pear12 = createEntity("pear", grape11); + auto orange13 = createEntity("orange", pear12); + auto orange14 = createEntity("orange", orange13); + + EXPECT_TRUE(gazebo::entitiesFromScopedName("watermelon", ecm).empty()); + + auto bananas = gazebo::entitiesFromScopedName("banana", ecm); + EXPECT_EQ(1u, bananas.size()); + EXPECT_NE(bananas.find(banana1), bananas.end()); + + auto oranges = gazebo::entitiesFromScopedName("orange", ecm); + EXPECT_EQ(3u, oranges.size()); + EXPECT_NE(oranges.find(orange2), oranges.end()); + EXPECT_NE(oranges.find(orange13), oranges.end()); + EXPECT_NE(oranges.find(orange14), oranges.end()); + + auto bananaOranges = gazebo::entitiesFromScopedName("banana::orange", ecm); + EXPECT_EQ(1u, bananaOranges.size()); + EXPECT_NE(bananaOranges.find(orange2), bananaOranges.end()); + + auto bananaGrapes = gazebo::entitiesFromScopedName("banana::grape", ecm); + EXPECT_EQ(2u, bananaGrapes.size()); + EXPECT_NE(bananaGrapes.find(grape7), bananaGrapes.end()); + EXPECT_NE(bananaGrapes.find(grape11), bananaGrapes.end()); + EXPECT_EQ(bananaGrapes.find(grape4), bananaGrapes.end()); + + auto grapePears = gazebo::entitiesFromScopedName("grape::pear", ecm); + EXPECT_EQ(3u, grapePears.size()); + EXPECT_NE(grapePears.find(pear5), grapePears.end()); + EXPECT_NE(grapePears.find(pear8), grapePears.end()); + EXPECT_NE(grapePears.find(pear12), grapePears.end()); + EXPECT_EQ(grapePears.find(pear10), grapePears.end()); + + auto grapePearPlums = + gazebo::entitiesFromScopedName("grape::pear::plum", ecm); + EXPECT_EQ(2u, grapePearPlums.size()); + EXPECT_NE(grapePearPlums.find(plum6), grapePearPlums.end()); + EXPECT_NE(grapePearPlums.find(plum9), grapePearPlums.end()); + EXPECT_EQ(grapePearPlums.find(plum3), grapePearPlums.end()); + + auto bananaOrangePlumGrapePearPlums = gazebo::entitiesFromScopedName( + "banana::orange::plum::grape::pear::plum", ecm); + EXPECT_EQ(1u, bananaOrangePlumGrapePearPlums.size()); + EXPECT_NE(bananaOrangePlumGrapePearPlums.find(plum6), + bananaOrangePlumGrapePearPlums.end()); + + auto bananaOrangeKiwiGrapePearPlums = gazebo::entitiesFromScopedName( + "banana::orange::kiwi::grape::pear::plum", ecm); + EXPECT_EQ(0u, bananaOrangeKiwiGrapePearPlums.size()); + + auto orangeOranges = gazebo::entitiesFromScopedName("orange::orange", ecm); + EXPECT_EQ(1u, orangeOranges.size()); + EXPECT_NE(orangeOranges.find(orange14), orangeOranges.end()); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/plot_3d/Plot3D.cc b/src/gui/plugins/plot_3d/Plot3D.cc index e194e6cb8d..45565bbc75 100644 --- a/src/gui/plugins/plot_3d/Plot3D.cc +++ b/src/gui/plugins/plot_3d/Plot3D.cc @@ -81,7 +81,7 @@ namespace ignition::gazebo::gui /// \brief Maximum number of points in the plot. When the plot reaches this /// size, older points are deleted. - public: int64_t maxPoints{1000}; + public: int maxPoints{1000}; /// \brief Protects variables that are updated by the user. public: std::mutex mutex; @@ -150,7 +150,7 @@ void Plot3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) auto ptsElem = _pluginElem->FirstChildElement("maximum_points"); if (nullptr != ptsElem && nullptr != ptsElem->GetText()) { - ptsElem->QueryInt64Text(&this->dataPtr->maxPoints); + ptsElem->QueryIntText(&this->dataPtr->maxPoints); this->MaxPointsChanged(); } } @@ -173,12 +173,20 @@ void Plot3D::ClearPlot() ////////////////////////////////////////////////// void Plot3D::Update(const UpdateInfo &, EntityComponentManager &_ecm) { + std::lock_guard lock(this->dataPtr->mutex); bool newTarget{false}; // New target by name, get entity if (this->dataPtr->targetNameDirty) { - auto entity = entityFromScopedName(this->dataPtr->targetName, _ecm); + auto entities = entitiesFromScopedName(this->dataPtr->targetName, _ecm); + if (entities.empty()) + { + // Keep trying + return; + } + + Entity entity = *(entities.begin()); if (kNullEntity == entity) { @@ -290,6 +298,7 @@ Entity Plot3D::TargetEntity() const ///////////////////////////////////////////////// void Plot3D::SetTargetEntity(Entity _entity) { + std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->targetEntity = _entity; this->dataPtr->targetEntityDirty = true; this->TargetEntityChanged(); @@ -309,6 +318,7 @@ QString Plot3D::TargetName() const ///////////////////////////////////////////////// void Plot3D::SetTargetName(const QString &_targetName) { + std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->targetName = _targetName.toStdString(); this->dataPtr->targetNameDirty = true; this->TargetNameChanged(); @@ -323,6 +333,7 @@ bool Plot3D::Locked() const ///////////////////////////////////////////////// void Plot3D::SetLocked(bool _locked) { + std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->locked = _locked; this->LockedChanged(); } @@ -339,6 +350,7 @@ QVector3D Plot3D::Offset() const ///////////////////////////////////////////////// void Plot3D::SetOffset(const QVector3D &_offset) { + std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->offset.Set(_offset.x(), _offset.y(), _offset.z()); this->OffsetChanged(); } @@ -355,6 +367,7 @@ QVector3D Plot3D::Color() const ///////////////////////////////////////////////// void Plot3D::SetColor(const QVector3D &_color) { + std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->color.Set(_color.x(), _color.y(), _color.z()); this->ColorChanged(); } @@ -368,6 +381,7 @@ double Plot3D::MinDistance() const ///////////////////////////////////////////////// void Plot3D::SetMinDistance(double _minDistance) { + std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->minDistance = _minDistance; this->MinDistanceChanged(); } @@ -381,6 +395,7 @@ int Plot3D::MaxPoints() const ///////////////////////////////////////////////// void Plot3D::SetMaxPoints(int _maxPoints) { + std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->maxPoints = _maxPoints; this->MaxPointsChanged(); } diff --git a/src/gui/plugins/plot_3d/Plot3D.hh b/src/gui/plugins/plot_3d/Plot3D.hh index a46b90a1f0..faf4466024 100644 --- a/src/gui/plugins/plot_3d/Plot3D.hh +++ b/src/gui/plugins/plot_3d/Plot3D.hh @@ -53,7 +53,7 @@ namespace gui /// /// * `` (optional): The minimum distance between points to /// plot. A new point will not be plotted until the entity has moved beyond - /// this distance from the previous point. Defaults to 5 cm. + /// this distance from the previous point. Defaults to 0.05 m. /// /// * ` (optional)`: Maximum number of points on the plot. /// After this number is reached, the older points start being deleted. diff --git a/src/gui/plugins/plot_3d/Plot3D.qml b/src/gui/plugins/plot_3d/Plot3D.qml index acbbf1708c..1fbc9e2f6a 100644 --- a/src/gui/plugins/plot_3d/Plot3D.qml +++ b/src/gui/plugins/plot_3d/Plot3D.qml @@ -108,198 +108,201 @@ Rectangle { } } } - GridLayout { + ColumnLayout { anchors.top: header.bottom anchors.bottom: parent.bottom anchors.left: parent.left anchors.right: parent.right - columns: 6 - Text { - text: "Offset" + GridLayout { Layout.fillWidth: true - Layout.row: 0 - Layout.column: 0 - Layout.columnSpan: 6 - color: "dimgrey" - font.bold: true - leftPadding: 5 - } + columns: 6 - Text { - text: "X (m)" - color: "dimgrey" - Layout.row: 1 - Layout.column: 0 - leftPadding: 5 - } - IgnSpinBox { - id: x - Layout.fillWidth: true - Layout.row: 1 - Layout.column: 1 - value: Plot3D.offset.x - maximumValue: 1000000 - minimumValue: -1000000 - decimals: 2 - stepSize: 0.01 - onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) - } - Text { - text: "Y (m)" - color: "dimgrey" - Layout.row: 1 - Layout.column: 2 - leftPadding: 5 - } - IgnSpinBox { - id: y - Layout.fillWidth: true - Layout.row: 1 - Layout.column: 3 - value: Plot3D.offset.y - maximumValue: 1000000 - minimumValue: -1000000 - decimals: 2 - stepSize: 0.01 - onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) - } - Text { - text: "Z (m)" - color: "dimgrey" - Layout.row: 1 - Layout.column: 4 - leftPadding: 5 - } - IgnSpinBox { - id: z - Layout.fillWidth: true - Layout.row: 1 - Layout.column: 5 - value: Plot3D.offset.z - maximumValue: 1000000 - minimumValue: -1000000 - decimals: 2 - stepSize: 0.01 - onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) - } + Text { + text: "Offset" + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 0 + Layout.columnSpan: 6 + color: "dimgrey" + font.bold: true + leftPadding: 5 + } - Text { - text: "Color" - Layout.fillWidth: true - Layout.row: 2 - Layout.column: 0 - Layout.columnSpan: 6 - color: "dimgrey" - font.bold: true - leftPadding: 5 - } + Text { + text: "X (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: x + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 1 + value: Plot3D.offset.x + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) + } + Text { + text: "Y (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 2 + leftPadding: 5 + } + IgnSpinBox { + id: y + Layout.fillWidth: true + value: Plot3D.offset.y + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) + } + Text { + text: "Z (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 4 + leftPadding: 5 + } + IgnSpinBox { + id: z + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 5 + value: Plot3D.offset.z + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) + } - Text { - text: "R" - color: "dimgrey" - Layout.row: 3 - Layout.column: 0 - leftPadding: 5 - } + Text { + text: "Color" + Layout.fillWidth: true + Layout.row: 2 + Layout.column: 0 + Layout.columnSpan: 6 + color: "dimgrey" + font.bold: true + leftPadding: 5 + } - IgnSpinBox { - id: r - Layout.fillWidth: true - Layout.row: 3 - Layout.column: 1 - value: Plot3D.color.x - maximumValue: 1.00 - minimumValue: 0.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) - } + Text { + text: "R" + color: "dimgrey" + Layout.row: 3 + Layout.column: 0 + leftPadding: 5 + } - Text { - text: "G" - Layout.row: 3 - Layout.column: 2 - color: "dimgrey" - leftPadding: 5 - } + IgnSpinBox { + id: r + Layout.fillWidth: true + Layout.row: 3 + Layout.column: 1 + value: Plot3D.color.x + maximumValue: 1.00 + minimumValue: 0.00 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) + } - IgnSpinBox { - id: g - Layout.fillWidth: true - Layout.row: 3 - Layout.column: 3 - value: Plot3D.color.y - maximumValue: 1.00 - minimumValue: 0.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) - } + Text { + text: "G" + Layout.row: 3 + Layout.column: 2 + color: "dimgrey" + leftPadding: 5 + } - Text { - text: "B" - Layout.row: 3 - Layout.column: 4 - color: "dimgrey" - leftPadding: 5 - } + IgnSpinBox { + id: g + Layout.fillWidth: true + Layout.row: 3 + Layout.column: 3 + value: Plot3D.color.y + maximumValue: 1.00 + minimumValue: 0.00 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) + } - IgnSpinBox { - id: b - Layout.fillWidth: true - Layout.row: 3 - Layout.column: 5 - value: Plot3D.color.z - maximumValue: 1.00 - minimumValue: 0.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) - } + Text { + text: "B" + Layout.row: 3 + Layout.column: 4 + color: "dimgrey" + leftPadding: 5 + } - Text { - text: "Min distance between points (m)" - color: "dimgrey" - Layout.row: 4 - Layout.column: 0 - Layout.columnSpan: 3 - leftPadding: 5 - } - IgnSpinBox { - id: minDist - Layout.fillWidth: true - Layout.row: 4 - Layout.column: 3 - Layout.columnSpan: 3 - value: Plot3D.minDistance - maximumValue: 1000000 - minimumValue: 0 - decimals: 6 - stepSize: 0.01 - onEditingFinished: Plot3D.SetMinDistance(minDist.value) + IgnSpinBox { + id: b + Layout.fillWidth: true + Layout.row: 3 + Layout.column: 5 + value: Plot3D.color.z + maximumValue: 1.00 + minimumValue: 0.00 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) + } } - Text { - text: "Max points" - color: "dimgrey" - Layout.row: 5 - Layout.column: 0 - Layout.columnSpan: 3 - leftPadding: 5 - } - IgnSpinBox { - id: maxPoints + GridLayout { Layout.fillWidth: true - Layout.row: 5 - Layout.column: 3 - Layout.columnSpan: 3 - value: Plot3D.maxPoints - maximumValue: 1000000 - minimumValue: 0 - decimals: 0 - stepSize: 100 - onEditingFinished: Plot3D.SetMaxPoints(maxPoints.value) + columns: 2 + + Text { + text: "Min distance between points (m)" + color: "dimgrey" + Layout.row: 0 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: minDist + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 1 + value: Plot3D.minDistance + maximumValue: 1000000 + minimumValue: 0 + decimals: 6 + stepSize: 0.01 + onEditingFinished: Plot3D.SetMinDistance(minDist.value) + } + + Text { + text: "Max points" + color: "dimgrey" + Layout.row: 1 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: maxPoints + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 1 + value: Plot3D.maxPoints + maximumValue: 1000000 + minimumValue: 0 + decimals: 0 + stepSize: 100 + onEditingFinished: Plot3D.SetMaxPoints(maxPoints.value) + } } // Bottom spacer diff --git a/src/gui/plugins/plot_3d/Plot3D_TEST.cc b/src/gui/plugins/plot_3d/Plot3D_TEST.cc index 1ea29be3b6..159fb80825 100644 --- a/src/gui/plugins/plot_3d/Plot3D_TEST.cc +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -102,12 +102,12 @@ TEST_F(Plot3D, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) ASSERT_EQ(plugins.size(), 1); auto plugin = plugins[0]; - EXPECT_EQ(plugin->Title(), "Plot3D!"); - EXPECT_EQ(plugin->TargetEntity(), gazebo::kNullEntity); - EXPECT_EQ(plugin->TargetName(), QString("banana")) + EXPECT_EQ("Plot3D!", plugin->Title()); + EXPECT_EQ(gazebo::kNullEntity, plugin->TargetEntity()); + EXPECT_EQ(QString("banana"), plugin->TargetName()) << plugin->TargetName().toStdString(); - EXPECT_EQ(plugin->Color(), QVector3D(0.1, 0.2, 0.3)); - EXPECT_EQ(plugin->Offset(), QVector3D(1, 2, 3)); + EXPECT_EQ(QVector3D(0.1, 0.2, 0.3), plugin->Color()); + EXPECT_EQ(QVector3D(1, 2, 3), plugin->Offset()); EXPECT_TRUE(plugin->Locked()); // Cleanup From a743c908ec524fc90e3e05cb27d9c7075be68f4d Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 27 Jul 2021 19:08:04 -0700 Subject: [PATCH 08/10] add relative to Signed-off-by: Louise Poubel --- include/ignition/gazebo/Util.hh | 10 +++- src/Util.cc | 17 ++++++- src/Util_TEST.cc | 83 ++++++++++++++------------------- 3 files changed, 58 insertions(+), 52 deletions(-) diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index f9e72b110e..08e30aa62b 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -52,7 +52,8 @@ namespace ignition bool _includePrefix = true); /// \brief Helper function to get an entity given its scoped name. - /// The scope may start at any level. For example, in this hierarchy: + /// The scope may start at any level by default. For example, in this + /// hierarchy: /// /// world_name /// model_name @@ -66,11 +67,16 @@ namespace ignition /// /// \param[in] _scopedName Entity's scoped name. /// \param[in] _ecm Immutable reference to ECM. - /// \param[in] _delim Delimiter between names, defaults to "::". + /// \param[in] _relativeTo Entity that the scoped name is relative to. The + /// scoped name does not include the name of this entity. If not provided, + /// the scoped name could be relative to any entity. + /// \param[in] _delim Delimiter between names, defaults to "::", it can't + /// be empty. /// \return The entity or kNullEntity if no entity was found with that /// name. std::unordered_set IGNITION_GAZEBO_VISIBLE entitiesFromScopedName( const std::string &_scopedName, const EntityComponentManager &_ecm, + Entity _relativeTo = kNullEntity, const std::string &_delim = "::"); /// \brief Generally, each entity will be of some specific high-level type, diff --git a/src/Util.cc b/src/Util.cc index 1401673f3c..1526f05256 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -136,8 +136,15 @@ std::string scopedName(const Entity &_entity, ////////////////////////////////////////////////// std::unordered_set entitiesFromScopedName( const std::string &_scopedName, const EntityComponentManager &_ecm, - const std::string &_delim) + Entity _relativeTo, const std::string &_delim) { + if (_delim.empty()) + { + ignwarn << "Can't process scoped name [" << _scopedName + << "] with empty delimiter." << std::endl; + return {}; + } + // Split names std::vector names; size_t pos1 = 0; @@ -150,7 +157,15 @@ std::unordered_set entitiesFromScopedName( } names.push_back(_scopedName.substr(pos1, _scopedName.size()-pos1)); + // Holds current entities that match and is updated for each name std::vector resVector; + + // If there's an entity we're relative to, treat it as the first level result + if (_relativeTo != kNullEntity) + { + resVector = {_relativeTo}; + } + for (const auto &name : names) { std::vector current; diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index fa2bba9a4b..e041e5086d 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -237,6 +237,7 @@ TEST_F(UtilTest, EntitiesFromScopedName) // - pear 12 // - orange 13 // - orange 14 + // - pear 15 auto createEntity = [&ecm](const std::string &_name, Entity _parent) -> Entity { @@ -262,56 +263,40 @@ TEST_F(UtilTest, EntitiesFromScopedName) auto pear12 = createEntity("pear", grape11); auto orange13 = createEntity("orange", pear12); auto orange14 = createEntity("orange", orange13); + auto pear15 = createEntity("pear", grape11); - EXPECT_TRUE(gazebo::entitiesFromScopedName("watermelon", ecm).empty()); - - auto bananas = gazebo::entitiesFromScopedName("banana", ecm); - EXPECT_EQ(1u, bananas.size()); - EXPECT_NE(bananas.find(banana1), bananas.end()); - - auto oranges = gazebo::entitiesFromScopedName("orange", ecm); - EXPECT_EQ(3u, oranges.size()); - EXPECT_NE(oranges.find(orange2), oranges.end()); - EXPECT_NE(oranges.find(orange13), oranges.end()); - EXPECT_NE(oranges.find(orange14), oranges.end()); - - auto bananaOranges = gazebo::entitiesFromScopedName("banana::orange", ecm); - EXPECT_EQ(1u, bananaOranges.size()); - EXPECT_NE(bananaOranges.find(orange2), bananaOranges.end()); - - auto bananaGrapes = gazebo::entitiesFromScopedName("banana::grape", ecm); - EXPECT_EQ(2u, bananaGrapes.size()); - EXPECT_NE(bananaGrapes.find(grape7), bananaGrapes.end()); - EXPECT_NE(bananaGrapes.find(grape11), bananaGrapes.end()); - EXPECT_EQ(bananaGrapes.find(grape4), bananaGrapes.end()); - - auto grapePears = gazebo::entitiesFromScopedName("grape::pear", ecm); - EXPECT_EQ(3u, grapePears.size()); - EXPECT_NE(grapePears.find(pear5), grapePears.end()); - EXPECT_NE(grapePears.find(pear8), grapePears.end()); - EXPECT_NE(grapePears.find(pear12), grapePears.end()); - EXPECT_EQ(grapePears.find(pear10), grapePears.end()); - - auto grapePearPlums = - gazebo::entitiesFromScopedName("grape::pear::plum", ecm); - EXPECT_EQ(2u, grapePearPlums.size()); - EXPECT_NE(grapePearPlums.find(plum6), grapePearPlums.end()); - EXPECT_NE(grapePearPlums.find(plum9), grapePearPlums.end()); - EXPECT_EQ(grapePearPlums.find(plum3), grapePearPlums.end()); - - auto bananaOrangePlumGrapePearPlums = gazebo::entitiesFromScopedName( - "banana::orange::plum::grape::pear::plum", ecm); - EXPECT_EQ(1u, bananaOrangePlumGrapePearPlums.size()); - EXPECT_NE(bananaOrangePlumGrapePearPlums.find(plum6), - bananaOrangePlumGrapePearPlums.end()); - - auto bananaOrangeKiwiGrapePearPlums = gazebo::entitiesFromScopedName( - "banana::orange::kiwi::grape::pear::plum", ecm); - EXPECT_EQ(0u, bananaOrangeKiwiGrapePearPlums.size()); - - auto orangeOranges = gazebo::entitiesFromScopedName("orange::orange", ecm); - EXPECT_EQ(1u, orangeOranges.size()); - EXPECT_NE(orangeOranges.find(orange14), orangeOranges.end()); + auto checkEntities = [&ecm](const std::string &_scopedName, Entity _relativeTo, + const std::unordered_set &_result, const std::string &_delim) + { + auto res = gazebo::entitiesFromScopedName(_scopedName, ecm, _relativeTo, + _delim); + EXPECT_EQ(_result.size(), res.size()) << _scopedName; + + for (auto it : _result) + { + EXPECT_NE(res.find(it), res.end()) << it << " " << _scopedName; + } + }; + + checkEntities("watermelon", kNullEntity, {}, "::"); + checkEntities("banana", kNullEntity, {banana1}, "::"); + checkEntities("orange", kNullEntity, {orange2, orange13, orange14}, ":"); + checkEntities("banana::orange", kNullEntity, {orange2}, "::"); + checkEntities("banana::grape", kNullEntity, {grape7, grape11}, "::"); + checkEntities("grape/pear", kNullEntity, {pear5, pear8, pear12, pear15}, "/"); + checkEntities("grape...pear...plum", kNullEntity, {plum6, plum9}, "..."); + checkEntities( + "banana::orange::plum::grape::pear::plum", kNullEntity, {plum6}, "::"); + checkEntities( + "banana::orange::kiwi::grape::pear::plum", kNullEntity, {}, "::"); + checkEntities("orange+orange", kNullEntity, {orange14}, "+"); + checkEntities("orange", banana1, {orange2}, "::"); + checkEntities("grape", banana1, {grape7, grape11}, "::"); + checkEntities("orange", orange2, {}, "::"); + checkEntities("orange", orange13, {orange14}, "::"); + checkEntities("grape::pear::plum", plum3, {plum6}, "::"); + checkEntities("pear", grape11, {pear12, pear15}, "=="); + checkEntities("plum=pear", pear8, {pear10}, "="); } ///////////////////////////////////////////////// From 9e6f743030ad3dcc74b766a6a4f0a7ac21454c70 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 28 Jul 2021 18:00:34 -0700 Subject: [PATCH 09/10] PR feedback Signed-off-by: Louise Poubel --- include/ignition/gazebo/Util.hh | 5 +++-- src/Util_TEST.cc | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 08e30aa62b..a76f919bde 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_UTIL_HH_ #include +#include #include #include @@ -72,8 +73,8 @@ namespace ignition /// the scoped name could be relative to any entity. /// \param[in] _delim Delimiter between names, defaults to "::", it can't /// be empty. - /// \return The entity or kNullEntity if no entity was found with that - /// name. + /// \return All entities that match the scoped name and relative to + /// requirements, or an empty set otherwise. std::unordered_set IGNITION_GAZEBO_VISIBLE entitiesFromScopedName( const std::string &_scopedName, const EntityComponentManager &_ecm, Entity _relativeTo = kNullEntity, diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index e041e5086d..4054ed511b 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -265,8 +265,9 @@ TEST_F(UtilTest, EntitiesFromScopedName) auto orange14 = createEntity("orange", orange13); auto pear15 = createEntity("pear", grape11); - auto checkEntities = [&ecm](const std::string &_scopedName, Entity _relativeTo, - const std::unordered_set &_result, const std::string &_delim) + auto checkEntities = [&ecm](const std::string &_scopedName, + Entity _relativeTo, const std::unordered_set &_result, + const std::string &_delim) { auto res = gazebo::entitiesFromScopedName(_scopedName, ecm, _relativeTo, _delim); From 362570b6f6c60fb0779c4cc7623e4f8b29a18151 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 28 Jul 2021 18:17:53 -0700 Subject: [PATCH 10/10] Changes to backport to Citadel Signed-off-by: Louise Poubel --- examples/worlds/plot_3d.sdf | 6 +++--- src/Util_TEST.cc | 2 +- src/gui/plugins/plot_3d/Plot3D_TEST.cc | 5 ++++- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/examples/worlds/plot_3d.sdf b/examples/worlds/plot_3d.sdf index fe482cddfe..afa33866c3 100644 --- a/examples/worlds/plot_3d.sdf +++ b/examples/worlds/plot_3d.sdf @@ -82,7 +82,7 @@ Upper link plot - Double pendulum::upper_link + Double_pendulum::upper_link 0 0 1 0 0 1 20 @@ -94,7 +94,7 @@ Lower link plot - Double pendulum::lower_link + Double_pendulum::lower_link 0 1 0 0 0 1 40 @@ -144,7 +144,7 @@ - Double pendulum + Double_pendulum https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 4054ed511b..8f909cb8d1 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -219,7 +219,7 @@ TEST(UtilTest, ScopedName) } ///////////////////////////////////////////////// -TEST_F(UtilTest, EntitiesFromScopedName) +TEST(UtilTest, EntitiesFromScopedName) { EntityComponentManager ecm; diff --git a/src/gui/plugins/plot_3d/Plot3D_TEST.cc b/src/gui/plugins/plot_3d/Plot3D_TEST.cc index 159fb80825..e84ed4abf5 100644 --- a/src/gui/plugins/plot_3d/Plot3D_TEST.cc +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -40,7 +40,10 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/test_config.hh" -#include "../../GuiRunner.hh" +// Use this when forward-porting to v6 +// #include "../../GuiRunner.hh" +#include "ignition/gazebo/gui/GuiRunner.hh" + #include "Plot3D.hh" int g_argc = 1;