diff --git a/examples/worlds/plot_3d.sdf b/examples/worlds/plot_3d.sdf new file mode 100644 index 0000000000..afa33866c3 --- /dev/null +++ b/examples/worlds/plot_3d.sdf @@ -0,0 +1,152 @@ + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 5 -1.5 3 0 0.37 2.8 + + + + + + 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 + + + + diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 8dc1958585..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 @@ -51,6 +52,34 @@ namespace ignition const EntityComponentManager &_ecm, const std::string &_delim = "/", bool _includePrefix = true); + /// \brief Helper function to get an entity given its scoped name. + /// The scope may start at any level by default. 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] _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 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, + 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..1526f05256 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -59,8 +59,16 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { math::Pose3d worldPose(const Entity &_entity, const EntityComponentManager &_ecm) { + 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 = _ecm.Component(_entity)->Data(); + math::Pose3d pose = poseComp->Data(); auto p = _ecm.Component(_entity); while (p) { @@ -125,6 +133,64 @@ std::string scopedName(const Entity &_entity, return result; } +////////////////////////////////////////////////// +std::unordered_set entitiesFromScopedName( + const std::string &_scopedName, const EntityComponentManager &_ecm, + 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; + 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, pos1); + } + 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; + if (resVector.empty()) + { + current = _ecm.EntitiesByComponents(components::Name(name)); + } + else + { + 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)); + } + } + if (current.empty()) + return {}; + resVector = current; + } + + return std::unordered_set(resVector.begin(), resVector.end()); +} + ////////////////////////////////////////////////// ComponentTypeId entityTypeId(const Entity &_entity, const EntityComponentManager &_ecm) diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 5325be39d2..8f909cb8d1 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -218,6 +218,88 @@ TEST(UtilTest, ScopedName) EXPECT_EQ(kNullEntity, gazebo::worldEntity(kNullEntity, ecm)); } +///////////////////////////////////////////////// +TEST(UtilTest, EntitiesFromScopedName) +{ + EntityComponentManager 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 + // - pear 15 + + 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); + auto pear15 = createEntity("pear", grape11); + + 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}, "="); +} + ///////////////////////////////////////////////// TEST(UtilTest, EntityTypeId) { 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..3ff591a861 --- /dev/null +++ b/src/gui/plugins/plot_3d/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(Plot3D + SOURCES + 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 new file mode 100644 index 0000000000..45565bbc75 --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.cc @@ -0,0 +1,405 @@ +/* + * 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::gazebo::gui +{ + /// \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 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. + public: double minDistance{0.05}; + + /// \brief Maximum number of points in the plot. When the plot reaches this + /// size, older points are deleted. + public: int maxPoints{1000}; + + /// \brief Protects variables that are updated by the user. + public: std::mutex mutex; + }; +} + +using namespace ignition; +using namespace ignition::gazebo; +using namespace ignition::gazebo::gui; + +///////////////////////////////////////////////// +Plot3D::Plot3D() + : GuiSystem(), dataPtr(std::make_unique()) +{ + qRegisterMetaType("Entity"); +} + +///////////////////////////////////////////////// +Plot3D::~Plot3D() +{ + this->ClearPlot(); +} + +///////////////////////////////////////////////// +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->SetLocked(true); + } + + auto offsetElem = _pluginElem->FirstChildElement("offset"); + if (nullptr != offsetElem && nullptr != offsetElem->GetText()) + { + std::stringstream offsetStr; + offsetStr << std::string(offsetElem->GetText()); + offsetStr >> this->dataPtr->offset; + this->OffsetChanged(); + } + + auto colorElem = _pluginElem->FirstChildElement("color"); + if (nullptr != colorElem && nullptr != colorElem->GetText()) + { + 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->QueryIntText(&this->dataPtr->maxPoints); + this->MaxPointsChanged(); + } + } + + ignition::gui::App()->findChild< + 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) +{ + std::lock_guard lock(this->dataPtr->mutex); + bool newTarget{false}; + + // New target by name, get entity + if (this->dataPtr->targetNameDirty) + { + auto entities = entitiesFromScopedName(this->dataPtr->targetName, _ecm); + if (entities.empty()) + { + // Keep trying + return; + } + + Entity entity = *(entities.begin()); + + 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) + { + this->ClearPlot(); + + // 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); + math::Pose3d offsetPose; + offsetPose.Set(this->dataPtr->offset, math::Vector3d::Zero); + + auto point = (pose * offsetPose).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 + 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) +{ + std::lock_guard lock(this->dataPtr->mutex); + 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) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->targetName = _targetName.toStdString(); + this->dataPtr->targetNameDirty = true; + this->TargetNameChanged(); +} + +///////////////////////////////////////////////// +bool Plot3D::Locked() const +{ + return this->dataPtr->locked; +} + +///////////////////////////////////////////////// +void Plot3D::SetLocked(bool _locked) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->locked = _locked; + this->LockedChanged(); +} + +///////////////////////////////////////////////// +QVector3D Plot3D::Offset() const +{ + return QVector3D( + this->dataPtr->offset.X(), + this->dataPtr->offset.Y(), + this->dataPtr->offset.Z()); +} + +///////////////////////////////////////////////// +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(); +} + +///////////////////////////////////////////////// +QVector3D Plot3D::Color() const +{ + return QVector3D( + this->dataPtr->color.R(), + this->dataPtr->color.G(), + this->dataPtr->color.B()); +} + +///////////////////////////////////////////////// +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(); +} + +///////////////////////////////////////////////// +double Plot3D::MinDistance() const +{ + return this->dataPtr->minDistance; +} + +///////////////////////////////////////////////// +void Plot3D::SetMinDistance(double _minDistance) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->minDistance = _minDistance; + this->MinDistanceChanged(); +} + +///////////////////////////////////////////////// +int Plot3D::MaxPoints() const +{ + return this->dataPtr->maxPoints; +} + +///////////////////////////////////////////////// +void Plot3D::SetMaxPoints(int _maxPoints) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->maxPoints = _maxPoints; + this->MaxPointsChanged(); +} + +// Register this plugin +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 new file mode 100644 index 0000000000..faf4466024 --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.hh @@ -0,0 +1,228 @@ +/* + * 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 +{ +namespace gui +{ + class Plot3DPrivate; + + /// \brief Plot the trajectory of an entity into the 3D scene. + /// + /// 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 + /// + /// * `` (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 0.05 m. + /// + /// * ` (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 + + /// \brief Target entity + Q_PROPERTY( + Entity targetEntity + READ TargetEntity + WRITE SetTargetEntity + NOTIFY TargetEntityChanged + ) + + /// \brief Target entity scoped name + Q_PROPERTY( + QString targetName + READ TargetName + WRITE SetTargetName + NOTIFY TargetNameChanged + ) + + /// \brief Whether the plugin is locked on an entity + Q_PROPERTY( + bool locked + READ Locked + WRITE SetLocked + NOTIFY LockedChanged + ) + + /// \brief XYZ offset to the entity's origin to plot + Q_PROPERTY( + QVector3D offset + READ Offset + WRITE SetOffset + NOTIFY OffsetChanged + ) + + /// \brief Plot line color + Q_PROPERTY( + QVector3D color + READ Color + WRITE SetColor + NOTIFY ColorChanged + ) + + /// \brief Minimum distance between plot points + Q_PROPERTY( + double minDistance + READ MinDistance + WRITE SetMinDistance + NOTIFY MinDistanceChanged + ) + + /// \brief Maximum number of total points on the plot + Q_PROPERTY( + int maxPoints + READ MaxPoints + WRITE SetMaxPoints + NOTIFY MaxPointsChanged + ) + + /// \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 plugin is currently locked on a target. + /// \return True for locked + public: Q_INVOKABLE bool Locked() const; + + /// \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 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 the 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 Notify that the color has changed. + signals: void ColorChanged(); + + /// \brief Get the minimum distance between points. + /// \return The current minimum distance. + public: Q_INVOKABLE double MinDistance() const; + + /// \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 number of points. + /// \return The current maximum points. + public: Q_INVOKABLE int MaxPoints() const; + + /// \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. + 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; + }; +} +} +} + +#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..1fbc9e2f6a --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.qml @@ -0,0 +1,317 @@ +/* + * 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: 320 + 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.empty ? "No entity selected" : 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 + } + } + } + ColumnLayout { + anchors.top: header.bottom + anchors.bottom: parent.bottom + anchors.left: parent.left + anchors.right: parent.right + + GridLayout { + Layout.fillWidth: true + columns: 6 + + Text { + text: "Offset" + Layout.fillWidth: true + Layout.row: 0 + 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: "Color" + Layout.fillWidth: true + Layout.row: 2 + Layout.column: 0 + Layout.columnSpan: 6 + color: "dimgrey" + font.bold: true + leftPadding: 5 + } + + Text { + text: "R" + color: "dimgrey" + Layout.row: 3 + Layout.column: 0 + 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: "G" + Layout.row: 3 + Layout.column: 2 + 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)) + } + + Text { + text: "B" + Layout.row: 3 + Layout.column: 4 + color: "dimgrey" + leftPadding: 5 + } + + 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)) + } + } + + GridLayout { + Layout.fillWidth: true + 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 + 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 + + 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..e84ed4abf5 --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -0,0 +1,118 @@ +/* + * 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" + +// Use this when forward-porting to v6 +// #include "../../GuiRunner.hh" +#include "ignition/gazebo/gui/GuiRunner.hh" + +#include "Plot3D.hh" + +int g_argc = 1; +char* g_argv[] = +{ + reinterpret_cast(const_cast("dummy")), +}; + +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("Plot3D!", plugin->Title()); + EXPECT_EQ(gazebo::kNullEntity, plugin->TargetEntity()); + EXPECT_EQ(QString("banana"), plugin->TargetName()) + << plugin->TargetName().toStdString(); + EXPECT_EQ(QVector3D(0.1, 0.2, 0.3), plugin->Color()); + EXPECT_EQ(QVector3D(1, 2, 3), plugin->Offset()); + 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());