From 52d801a51016ac81a2e88787b749b56213ea49bb Mon Sep 17 00:00:00 2001 From: jshep1 Date: Wed, 8 Apr 2020 11:16:16 -0700 Subject: [PATCH 01/18] Add skeleton shapes plugin --- src/gui/plugins/CMakeLists.txt | 1 + src/gui/plugins/shapes/CMakeLists.txt | 6 ++ src/gui/plugins/shapes/Shapes.cc | 90 +++++++++++++++++++++ src/gui/plugins/shapes/Shapes.hh | 60 ++++++++++++++ src/gui/plugins/shapes/Shapes.qml | 112 ++++++++++++++++++++++++++ src/gui/plugins/shapes/Shapes.qrc | 8 ++ src/gui/plugins/shapes/box.png | Bin 0 -> 863 bytes src/gui/plugins/shapes/cylinder.png | Bin 0 -> 983 bytes src/gui/plugins/shapes/sphere.png | Bin 0 -> 1449 bytes 9 files changed, 277 insertions(+) create mode 100644 src/gui/plugins/shapes/CMakeLists.txt create mode 100644 src/gui/plugins/shapes/Shapes.cc create mode 100644 src/gui/plugins/shapes/Shapes.hh create mode 100644 src/gui/plugins/shapes/Shapes.qml create mode 100644 src/gui/plugins/shapes/Shapes.qrc create mode 100644 src/gui/plugins/shapes/box.png create mode 100644 src/gui/plugins/shapes/cylinder.png create mode 100644 src/gui/plugins/shapes/sphere.png diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 0594c2c183..a1d479e215 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -85,6 +85,7 @@ add_subdirectory(component_inspector) add_subdirectory(entity_tree) add_subdirectory(grid_config) add_subdirectory(scene3d) +add_subdirectory(shapes) add_subdirectory(transform_control) add_subdirectory(video_recorder) add_subdirectory(view_angle) diff --git a/src/gui/plugins/shapes/CMakeLists.txt b/src/gui/plugins/shapes/CMakeLists.txt new file mode 100644 index 0000000000..9542555733 --- /dev/null +++ b/src/gui/plugins/shapes/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_gui_plugin(Shapes + SOURCES Shapes.cc + QT_HEADERS Shapes.hh +) + + diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc new file mode 100644 index 0000000000..7d6330f4ae --- /dev/null +++ b/src/gui/plugins/shapes/Shapes.cc @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2020 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 "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" + +#include "Shapes.hh" + +namespace ignition::gazebo +{ + class ShapesPrivate + { + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Mutex to protect mode + public: std::mutex mutex; + + /// \brief Transform control service name + public: std::string service; + }; +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +Shapes::Shapes() + : ignition::gui::Plugin(), + dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +Shapes::~Shapes() = default; + +///////////////////////////////////////////////// +void Shapes::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Shapes"; + + // For transform requests + this->dataPtr->service = "/gui/shapes"; +} + +///////////////////////////////////////////////// +void Shapes::OnMode(const QString &_mode) +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting transform mode" << std::endl; + }; + + ignition::msgs::StringMsg req; + req.set_data(_mode.toStdString()); + this->dataPtr->node.Request(this->dataPtr->service, req, cb); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::Shapes, + ignition::gui::Plugin) diff --git a/src/gui/plugins/shapes/Shapes.hh b/src/gui/plugins/shapes/Shapes.hh new file mode 100644 index 0000000000..c3dea2956d --- /dev/null +++ b/src/gui/plugins/shapes/Shapes.hh @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2020 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_SHAPES_HH_ +#define IGNITION_GAZEBO_GUI_SHAPES_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ + class ShapesPrivate; + + /// \brief Provides buttons for adding a box, sphere, or cylinder + /// to the scene + /// + /// ## Configuration + /// \ : Set the service to receive transform mode requests. + class Shapes : public ignition::gui::Plugin + { + Q_OBJECT + + /// \brief Constructor + public: Shapes(); + + /// \brief Destructor + public: ~Shapes() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + /// \brief Callback in Qt thread when mode changes. + /// \param[in] _mode New transform mode + public slots: void OnMode(const QString &_mode); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/shapes/Shapes.qml b/src/gui/plugins/shapes/Shapes.qml new file mode 100644 index 0000000000..76a5b1a5cb --- /dev/null +++ b/src/gui/plugins/shapes/Shapes.qml @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2020 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.Window 2.2 +import QtQuick.Controls 2.1 +import QtQuick.Controls.Material 2.2 +import QtQuick.Controls.Material.impl 2.2 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 + +ToolBar { + id: shapes + Layout.minimumWidth: 200 + Layout.minimumHeight: 100 + + property color snapTitle: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade900) : + Material.color(Material.Grey, Material.Shade200) + + property color snapItem: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade800) : + Material.color(Material.Grey, Material.Shade100) + + function windowWidth() { + return transformControl.Window.window ? (transformControl.Window.window.width) : 0 + } + + function windowHeight() { + return transformControl.Window.window ? (transformControl.Window.window.height) : 0 + } + + background: Rectangle { + color: "transparent" + } + + ButtonGroup { + id: group + } + + RowLayout { + spacing: 2 + ToolButton { + id: box + checkable: true + checked: true + ButtonGroup.group: group + ToolTip.text: "Box" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "box.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + } + } + ToolButton{ + id: sphere + checkable: true + ButtonGroup.group: group + ToolTip.text: "Sphere" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "sphere.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + } + } + ToolButton { + id: cylinder + checkable: true + ButtonGroup.group: group + ToolTip.text: "Cylinder" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "cylinder.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + } + } + } +} diff --git a/src/gui/plugins/shapes/Shapes.qrc b/src/gui/plugins/shapes/Shapes.qrc new file mode 100644 index 0000000000..0aad2596be --- /dev/null +++ b/src/gui/plugins/shapes/Shapes.qrc @@ -0,0 +1,8 @@ + + + Shapes.qml + box.png + sphere.png + cylinder.png + + diff --git a/src/gui/plugins/shapes/box.png b/src/gui/plugins/shapes/box.png new file mode 100644 index 0000000000000000000000000000000000000000..9064dfc0abbad8e2291b671116e3f1e46bef23fe GIT binary patch literal 863 zcmeAS@N?(olHy`uVBq!ia0vp^b|B2b3?yCTBg%jjOS+@4BLl<6e(pbstU$g(vPY0F z14ET614BbI1H;e%K>8&EL#Y7+!>a@a2CEqi4C48d;*Yuk)m{zo332`Z|NnKO!OfdD zfogBxzJ2G;olBQ4odg3Q8!Ldw!byl4I2X_R` z0<_}n*|TTPoCPYrbm{WNix-yn;M(d0RFt1M0E%ba4#vIR5tf$*5)niPnc6GoxfyKL`<>Big)DDWk(8F~$73tYZGbQYODrt8GfB*X_@<6MMA1F^{KuOo?PkiyTb2ny|CL@+JWgEcfZ))fbz6cLcow!Epd$~ zNl7e8wMs5Z1yT$~28L$329~;p#vz90R)$7a#s<0u=2iv3oc@92OHf?)F%lREJuUM3zRMMQvXH||Kqz}DLXtd$zB-}W5ju_` zG%5gX2||JC*LKR*B1EEa*YnvU6q+fr0-1{*I0gxm@1f-Y%2L`uh5mN@ed%^z`&76pHSd znAPI=puvC>gxP2`0fW&bd0dE0#WoY#W6eEpU*11#^QOVi)Gm!sR62Xo+Dz=1mF?o*xh#Jg34YJ`H-( z?)X?1I`107fyUzqC+QeMUjDz6gvxFO!O%?Q{_@p<9#(@y98Gh^0XvGSxWDte+P#k(NzUh5Z^ zRXJ9_mY|Zdd8qJcfbcU$am{UzKG;x<05Tg;$IE%Sx+tX8RFhe{pqaQ)+`FDWWC0mfw6kL5yqed~U32Zi>0u z`{6zN?2Rv~@`7&2i?q+sHY+)Z^73u6|uL9_Usbvr3*9mLP22| zErNUfq;{O8s&x2kdKymCu))S^6_ZMX= B0}cQH literal 0 HcmV?d00001 diff --git a/src/gui/plugins/shapes/sphere.png b/src/gui/plugins/shapes/sphere.png new file mode 100644 index 0000000000000000000000000000000000000000..824befa12efc96cda444668dd29443a342820880 GIT binary patch literal 1449 zcmV;a1y=frP)004R=004l4008;_004mL004C`008P>0026e000+nl3&F}00002 zVoOIv0RM-N%)bBt010qNS#tmY3ljhU3ljkVnw%H_000McNliru;0X;83?6Q@u%-Y2 z1e{4kK~!koty#^E+(Z!my6xF$S7?>Lmmn*F01;`!JV2fSB=*XiEd9wU0q$>?sm4UnXx^#zpuWk?yhbdLcKjAzyKf+y88(h z4L&pV&oVUx9+8FZ_o@H5o(SN2G7EfOL8OqtOL`3pGeCP#4(*Q}0N~Q`wNKB9zzJCx zc&8}%5F--q5rFN2pxFH1r{6uJ1zI$)=xB@^vU>&wZ*hH1_F=w7HYID+dPpnvD43ZS2O@^X01UwAA0_)#wAUy`(4J;dq?1b>;)y3-DK!FuCEWU^`9)qWRqy9TgMieRF zHLzX??v)W4z>`sHgmcO|;K&XHUJ&o&Ayj}MRi1!(V*hOh0v58>>tJ%Ag(`4bbd8<` z*1zh#n&zoqnoWDF+xiw@2+wID;_Gu?QXp*L;IQd`Wn+v}Ft3Nf7sgJOg^_5fk|TXb z?q3^5l!CuWT&{tpGoUOumOp-k(cvBY=tWt*nLlYq8G^%}&v(Oq+a%&&mjM8A+h;od2a>3j#jjOrQg?x@TMo zNEII;?K&#$26jk7A3vk`PXP%IE&We-QnA@C1mlIi^x{F0^Fg^mTw4b1@!DOy4aAQH z6`5E4{VB5fB>4dE%tVpNZ)1|Ro%bvdv!E#S~(n~ntHzR!pD)&dSz5kIAYwgwVP zUn)N%kfuPvH!0zO`dwxX`m+2nHHrC-zBa-KZ%zJE5=(=wD|Sd%yLyz*1AYhrCNdz@ zAqJZp{RY4=GdijYf(~Ene>XGg>k)%Z&qybLMAFTNALbbTmWRQDtfZi4`+WFuc05#M zCP+m&7UU?*fgh&=Y>DOp8RRD2ahEd}o-fjjH7PFOU2>9=Qe)o4Y**C+pBOWY_Z(XG z{9$vfhd20$$t>22?~^ty)kgdIlhS4I_-L{EkaGP&-Lz{OUYVI*{!!NbdJy8tTZvu% zTAops>(@V>eFddC))w3f{K2(51q6Wg&;NXxUWl0k4FFdTKD-m3O}?`EgD8^;^|nlS zlzC@#pEk4MLd(51$}ErUo2$Q^2mF<9axbkW-y6Q44h{AvR`rVBoX#UL4PHzD0G`mB z3h>>ayFC{IogLHb{NwNeKCW8JuOu+qUx%;una98H-JJKF{{cyyM}(}6@!HcdA00000NkvXXu0mjf DTMvDU literal 0 HcmV?d00001 From 704a1cafc524c072e5bc1bfc8727616a18b92df2 Mon Sep 17 00:00:00 2001 From: jshep1 Date: Tue, 14 Apr 2020 11:29:19 -0700 Subject: [PATCH 02/18] Connect to scene3d --- src/gui/plugins/scene3d/Scene3D.cc | 56 ++++++++++++++++++++++++++++++ src/gui/plugins/scene3d/Scene3D.hh | 15 ++++++++ src/gui/plugins/shapes/Shapes.cc | 7 ++-- src/gui/plugins/shapes/Shapes.qml | 3 ++ 4 files changed, 79 insertions(+), 2 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index ca23556f01..22cbe8131a 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -21,6 +21,8 @@ #include #include +#include + #include #include #include @@ -309,6 +311,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Follow service public: std::string viewAngleService; + + /// \brief Shapes service + public: std::string shapesService; }; } } @@ -1352,6 +1357,13 @@ void IgnRenderer::SetTransformMode(const std::string &_mode) } } +///////////////////////////////////////////////// +void IgnRenderer::SetModel(const std::string &_model) +{ + + +} + ///////////////////////////////////////////////// void IgnRenderer::SetRecordVideo(bool _record, const std::string &_format, const std::string &_savePath) @@ -1927,6 +1939,14 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignmsg << "View angle service on [" << this->dataPtr->viewAngleService << "]" << std::endl; + // shapes + this->dataPtr->shapesService = + "/gui/shapes"; + this->dataPtr->node.Advertise(this->dataPtr->shapesService, + &Scene3D::OnShapes, this); + ignmsg << "Shapes service on [" + << this->dataPtr->shapesService << "]" << std::endl; + ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->installEventFilter(this); } @@ -2018,6 +2038,36 @@ bool Scene3D::OnViewAngle(const msgs::Vector3d &_msg, return true; } +///////////////////////////////////////////////// +bool Scene3D::OnShapes(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + rendering::ScenePtr scene = this->dataPtr->renderUtil->Scene(); + rendering::VisualPtr rootVis = scene->RootVisual(); + + //renderWindow->SetModel(msgs::Convert(_msg)); + ignwarn << "Model " << msgs::Convert(_msg) << "\n"; + sdf::Root root; + root.Load(msgs::Convert(_msg)); + rendering::VisualPtr model = this->dataPtr->renderUtil->SceneManager().CreateModel(1000, *(root.ModelByIndex(0), 1)); + rootVis->AddChild(model); + /* + msgs::EntityFactory req; + req.set_sdf_filename(_drop.toStdString()); + req.set_allow_renaming(true); + msgs::Set(req.mutable_pose(), + math::Pose3d(pos.X(), pos.Y(), pos.Z(), 1, 0, 0, 0)); + + this->dataPtr->node.Request("/world/" + this->dataPtr->worldName + "/create", + req, cb); + */ + + _res.set_data(true); + return true; +} + ///////////////////////////////////////////////// void Scene3D::OnDropped(const QString &_drop, int _mouseX, int _mouseY) { @@ -2144,6 +2194,12 @@ void RenderWindowItem::SetTransformMode(const std::string &_mode) this->dataPtr->renderThread->ignRenderer.SetTransformMode(_mode); } +///////////////////////////////////////////////// +void RenderWindowItem::SetModel(const std::string &_model) +{ + this->dataPtr->renderThread->ignRenderer.SetModel(_model); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetRecordVideo(bool _record, const std::string &_format, const std::string &_savePath) diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 8a3d234ff0..91a9208b35 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -134,6 +134,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: bool OnViewAngle(const msgs::Vector3d &_msg, msgs::Boolean &_res); + /// \brief Callback for a shapes request + /// \param[in] _msg Request message to set a new shape. + /// \param[in] _res Response data. + /// \return True if the request is received. + private: bool OnShapes(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; @@ -171,6 +178,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _mode New transform mode to set to public: void SetTransformMode(const std::string &_mode); + /// \brief Set the model to hover. + /// \param[in] _mode Path to new model to load in for the user. + public: void SetModel(const std::string &_model); + /// \brief Set whether to record video /// \param[in] _record True to start video recording, false to stop. /// \param[in] _format Video encoding format: "mp4", "ogv" @@ -431,6 +442,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _mode New transform mode to set to public: void SetTransformMode(const std::string &_mode); + /// \brief Set the model to hover. + /// \param[in] _mode Path to new model to load in for the user. + public: void SetModel(const std::string &_model); + /// \brief Set whether to record video /// \param[in] _record True to start video recording, false to stop. /// \param[in] _format Video encoding format: "mp4", "ogv" diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index 7d6330f4ae..b2c7f4433a 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -25,6 +25,7 @@ #include #include +#include "ignition/gazebo/test_config.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -77,11 +78,13 @@ void Shapes::OnMode(const QString &_mode) [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) - ignerr << "Error setting transform mode" << std::endl; + ignerr << "Error setting shape" << std::endl; }; ignition::msgs::StringMsg req; - req.set_data(_mode.toStdString()); + + std::string path = std::string(PROJECT_SOURCE_PATH) + "/src/gui/plugins/shapes/" + _mode.toStdString() + ".sdf"; + req.set_data(path); this->dataPtr->node.Request(this->dataPtr->service, req, cb); } diff --git a/src/gui/plugins/shapes/Shapes.qml b/src/gui/plugins/shapes/Shapes.qml index 76a5b1a5cb..c2cf537e04 100644 --- a/src/gui/plugins/shapes/Shapes.qml +++ b/src/gui/plugins/shapes/Shapes.qml @@ -70,6 +70,7 @@ ToolBar { sourceSize.height: 24; } onClicked: { + Shapes.OnMode("box") } } ToolButton{ @@ -88,6 +89,7 @@ ToolBar { sourceSize.height: 24; } onClicked: { + Shapes.OnMode("sphere") } } ToolButton { @@ -106,6 +108,7 @@ ToolBar { sourceSize.height: 24; } onClicked: { + Shapes.OnMode("cylinder") } } } From 4e12dcb57fda92255a786330529150799a608c1f Mon Sep 17 00:00:00 2001 From: jshep1 Date: Tue, 14 Apr 2020 11:56:33 -0700 Subject: [PATCH 03/18] Add literal sdf string --- src/gui/plugins/scene3d/Scene3D.cc | 4 +-- src/gui/plugins/shapes/Shapes.cc | 45 ++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+), 2 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 22cbe8131a..836fd5e346 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -2050,8 +2050,8 @@ bool Scene3D::OnShapes(const msgs::StringMsg &_msg, //renderWindow->SetModel(msgs::Convert(_msg)); ignwarn << "Model " << msgs::Convert(_msg) << "\n"; sdf::Root root; - root.Load(msgs::Convert(_msg)); - rendering::VisualPtr model = this->dataPtr->renderUtil->SceneManager().CreateModel(1000, *(root.ModelByIndex(0), 1)); + root.LoadSdfString(msgs::Convert(_msg)); + rendering::VisualPtr model = this->dataPtr->renderUtil->SceneManager().CreateModel(1000, *(root.ModelByIndex(0), this->dataPtr->)); rootVis->AddChild(model); /* msgs::EntityFactory req; diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index b2c7f4433a..381181e5e9 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -83,6 +83,51 @@ void Shapes::OnMode(const QString &_mode) ignition::msgs::StringMsg req; + std::string sdfString; + if (_mode == "box") + { + sdfString = " \ + \ + \ + 0 0 0.5 0 0 0 \ + \ + \ + \ + 1 \ + 0 \ + 0 \ + 1 \ + 0 \ + 1 \ + \ + 1.0 \ + \ + \ + \ + \ + 1 1 1 \ + \ + \ + \ + \ + \ + \ + 1 1 1 \ + \ + \ + \ + \ + \ + "; + } + else if (_mode == "sphere") + { + + } + else if (_mode == "cylinder") + { + + } std::string path = std::string(PROJECT_SOURCE_PATH) + "/src/gui/plugins/shapes/" + _mode.toStdString() + ".sdf"; req.set_data(path); this->dataPtr->node.Request(this->dataPtr->service, req, cb); From 5fc810004a10d2ba7a8dd2e5dbacfe92227da556 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Fri, 24 Apr 2020 19:04:39 -0700 Subject: [PATCH 04/18] Add framework logic for model placement Signed-off-by: John Shepherd --- .../ignition/gazebo/rendering/SceneManager.hh | 4 + src/gui/plugins/scene3d/Scene3D.cc | 135 +++++++++++++++--- src/gui/plugins/scene3d/Scene3D.hh | 3 + src/gui/plugins/shapes/Shapes.cc | 73 +++++----- src/rendering/SceneManager.cc | 15 ++ 5 files changed, 175 insertions(+), 55 deletions(-) diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 74dcba935c..30636933a0 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -64,6 +64,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _id World ID. public: void SetWorldId(Entity _id); + /// \brief Get the world's ID. + /// \return World ID. + public: Entity WorldId(); + /// \brief Create a model /// \param[in] _id Unique model id /// \param[in] _model Model sdf dom diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 836fd5e346..c7c0f02c81 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -208,6 +209,15 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Flag for indicating whether we are in view angle mode or not public: bool viewAngle = false; + /// \brief Flag for indicating whether we are in shapes mode or not + public: bool shapes = false; + + /// \brief Flag for indicating whether the user is currently placing a + /// model with the shapes plugin or not + public: bool placingModel = false; + + public: std::string model; + /// \brief The pose set during a view angle button press that holds /// the pose the camera should assume relative to the entit(y/ies). /// The vector (0, 0, 0) indicates to return the camera back to the home @@ -376,6 +386,9 @@ void IgnRenderer::Render() // Entity selection this->HandleEntitySelection(); + // Model placement + this->HandleModelPlacement(); + // reset follow mode if target node got removed if (!this->dataPtr->followTarget.empty()) { @@ -556,6 +569,61 @@ void IgnRenderer::Render() } } + // Shapes + // TODO move shapes functionality here to reside within render thread + { + IGN_PROFILE("IgnRenderer::Render Shapes"); + if (this->dataPtr->shapes) + { + ignwarn << "1\n"; + rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); + ignwarn << "2\n"; + rendering::VisualPtr rootVis = scene->RootVisual(); + ignwarn << "3\n"; + + sdf::Root root; + root.LoadSdfString(this->dataPtr->model); + ignwarn << "4\n"; + sdf::Model model2 = *(root.ModelByIndex(0)); + ignwarn << "6\n"; + ignwarn << "model name: " << model2.Name() << "\n"; + for (auto i = 0u; i < root.ModelCount(); i++) + { + ignwarn << "5\n"; + sdf::Model model = *(root.ModelByIndex(i)); + ignwarn << "6\n"; + ignwarn << "model name: " << model.Name() << "\n"; + this->dataPtr->renderUtil.SceneManager().CreateModel(1000, model, this->dataPtr->renderUtil.SceneManager().WorldId()); + for (auto j = 0u; j < model.LinkCount(); j++) + { + sdf::Link link = *(model.LinkByIndex(j)); + ignwarn << "link name: " << link.Name() << "\n"; + this->dataPtr->renderUtil.SceneManager().CreateLink(999, link, 1000); + for (auto k = 0u; k < link.VisualCount(); k++) + { + sdf::Visual visual = *(link.VisualByIndex(k)); + ignwarn << "visual name: " << visual.Name() << "\n"; + ignwarn << "geom " << static_cast(visual.Geom()->Type()) << "\n"; + this->dataPtr->renderUtil.SceneManager().CreateVisual(998, visual, 999); + } + } + } + this->dataPtr->placingModel = true; + ignwarn << "7\n"; + /* + msgs::EntityFactory req; + req.set_sdf_filename(_drop.toStdString()); + req.set_allow_renaming(true); + msgs::Set(req.mutable_pose(), + math::Pose3d(pos.X(), pos.Y(), pos.Z(), 1, 0, 0, 0)); + + this->dataPtr->node.Request("/world/" + this->dataPtr->worldName + "/create", + req, cb); + */ + this->dataPtr->shapes = false; + } + } + if (ignition::gui::App()) { ignition::gui::App()->sendEvent( @@ -712,6 +780,48 @@ void IgnRenderer::HandleKeyRelease(QKeyEvent *_e) } } +///////////////////////////////////////////////// +void IgnRenderer::HandleModelPlacement() +{ + if (this->dataPtr->placingModel) + { + ignwarn << "Getting render window\n"; + auto renderWindow = this->PluginItem()->findChild(); + ignwarn << "Forcing render window focus\n"; + renderWindow->forceActiveFocus(); + if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::MOVE) + { + ignwarn << "moving to " << this->ScreenToScene(this->dataPtr->mouseEvent.Pos()) << "\n"; + } + if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT) + { + // TODO spawn the object with the logic below + /* + ignition::msgs::Pose req; + req.set_name(node->Name()); + msgs::Set(req.mutable_position(), node->WorldPosition()); + msgs::Set(req.mutable_orientation(), node->WorldRotation()); + if (this->dataPtr->poseCmdService.empty()) + { + this->dataPtr->poseCmdService = "/world/" + this->worldName + + "/set_pose"; + } + this->dataPtr->node.Request(this->dataPtr->poseCmdService, req, cb); + ignwarn << "placing at " << this->ScreenToScene(this->dataPtr->mouseEvent.Pos()) << "\n"; + this->dataPtr->placingModel = false; + msgs::EntityFactory req; + req.set_sdf_filename(_drop.toStdString()); + req.set_allow_renaming(true); + msgs::Set(req.mutable_pose(), + math::Pose3d(pos.X(), pos.Y(), pos.Z(), 1, 0, 0, 0)); + + this->dataPtr->node.Request("/world/" + this->dataPtr->worldName + "/create", + req, cb); + */ + } + } +} + ///////////////////////////////////////////////// void IgnRenderer::HandleEntitySelection() { @@ -1360,8 +1470,9 @@ void IgnRenderer::SetTransformMode(const std::string &_mode) ///////////////////////////////////////////////// void IgnRenderer::SetModel(const std::string &_model) { - - + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->shapes = true; + this->dataPtr->model = _model; } ///////////////////////////////////////////////// @@ -2044,25 +2155,7 @@ bool Scene3D::OnShapes(const msgs::StringMsg &_msg, { auto renderWindow = this->PluginItem()->findChild(); - rendering::ScenePtr scene = this->dataPtr->renderUtil->Scene(); - rendering::VisualPtr rootVis = scene->RootVisual(); - - //renderWindow->SetModel(msgs::Convert(_msg)); - ignwarn << "Model " << msgs::Convert(_msg) << "\n"; - sdf::Root root; - root.LoadSdfString(msgs::Convert(_msg)); - rendering::VisualPtr model = this->dataPtr->renderUtil->SceneManager().CreateModel(1000, *(root.ModelByIndex(0), this->dataPtr->)); - rootVis->AddChild(model); - /* - msgs::EntityFactory req; - req.set_sdf_filename(_drop.toStdString()); - req.set_allow_renaming(true); - msgs::Set(req.mutable_pose(), - math::Pose3d(pos.X(), pos.Y(), pos.Z(), 1, 0, 0, 0)); - - this->dataPtr->node.Request("/world/" + this->dataPtr->worldName + "/create", - req, cb); - */ + renderWindow->SetModel(msgs::Convert(_msg)); _res.set_data(true); return true; diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 91a9208b35..99d37f9e6c 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -323,6 +323,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Handle entity selection requests private: void HandleEntitySelection(); + /// \brief Handle model placement requests + private: void HandleModelPlacement(); + /// \brief Retrieve the first point on a surface in the 3D scene hit by a /// ray cast from the given 2D screen coordinates. /// \param[in] _screenPos 2D coordinates on the screen, in pixels. diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index 381181e5e9..49203bf41c 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -86,39 +86,44 @@ void Shapes::OnMode(const QString &_mode) std::string sdfString; if (_mode == "box") { - sdfString = " \ - \ - \ - 0 0 0.5 0 0 0 \ - \ - \ - \ - 1 \ - 0 \ - 0 \ - 1 \ - 0 \ - 1 \ - \ - 1.0 \ - \ - \ - \ - \ - 1 1 1 \ - \ - \ - \ - \ - \ - \ - 1 1 1 \ - \ - \ - \ - \ - \ - "; + sdfString = std::string("" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + "" + "1" + "0" + "0" + "1" + "0" + "1" + "" + "1.0" + "" + "" + "" + "" + "1 1 1" + "" + "" + "" + "" + "" + "" + "1 1 1" + "" + "" + "" + "1 0 0 1" + "1 0 0 1" + "1 0 0 1" + "" + "" + "" + "" + ""); } else if (_mode == "sphere") { @@ -129,7 +134,7 @@ void Shapes::OnMode(const QString &_mode) } std::string path = std::string(PROJECT_SOURCE_PATH) + "/src/gui/plugins/shapes/" + _mode.toStdString() + ".sdf"; - req.set_data(path); + req.set_data(sdfString); this->dataPtr->node.Request(this->dataPtr->service, req, cb); } diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 505dc00372..a8591894b7 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -89,6 +89,12 @@ void SceneManager::SetWorldId(Entity _id) this->dataPtr->worldId = _id; } +///////////////////////////////////////////////// +Entity SceneManager::WorldId() +{ + return this->dataPtr->worldId; +} + ///////////////////////////////////////////////// rendering::VisualPtr SceneManager::CreateModel(Entity _id, const sdf::Model &_model, Entity _parentId) @@ -181,12 +187,14 @@ rendering::VisualPtr SceneManager::CreateLink(Entity _id, rendering::VisualPtr SceneManager::CreateVisual(Entity _id, const sdf::Visual &_visual, Entity _parentId) { + ignwarn << "1\n"; if (this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end()) { ignerr << "Entity with Id: [" << _id << "] already exists in the scene" << std::endl; return rendering::VisualPtr(); } + ignwarn << "2\n"; rendering::VisualPtr parent; if (_parentId != this->dataPtr->worldId) @@ -200,6 +208,7 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, } parent = it->second; } + ignwarn << "3\n"; if (!_visual.Geom()) return rendering::VisualPtr(); @@ -210,14 +219,17 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, name = parent->Name() + "::" + name; rendering::VisualPtr visualVis = this->dataPtr->scene->CreateVisual(name); visualVis->SetLocalPose(_visual.Pose()); + ignwarn << "4\n"; math::Vector3d scale = math::Vector3d::One; math::Pose3d localPose; rendering::GeometryPtr geom = this->LoadGeometry(*_visual.Geom(), scale, localPose); + ignwarn << "5\n"; if (geom) { + ignwarn << "6\n"; /// localPose is currently used to handle the normal vector in plane visuals /// In general, this can be used to store any local transforms between the /// parent Visual and geometry. @@ -257,6 +269,7 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, } else { + ignwarn << "7\n"; // meshes created by mesh loader may have their own materials // update/override their properties based on input sdf element values auto mesh = std::dynamic_pointer_cast(geom); @@ -312,7 +325,9 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, rendering::GeometryPtr geom{nullptr}; if (_geom.Type() == sdf::GeometryType::BOX) { + ignwarn << "Creating box\n"; geom = this->dataPtr->scene->CreateBox(); + ignwarn << "Created box\n"; scale = _geom.BoxShape()->Size(); } else if (_geom.Type() == sdf::GeometryType::CYLINDER) From 4c3af710d50fa41383a36f95623cdb900c621408 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Sat, 25 Apr 2020 17:08:29 -0700 Subject: [PATCH 05/18] Add hover mouse detection Signed-off-by: John Shepherd --- src/gui/plugins/scene3d/GzScene3D.qml | 3 + src/gui/plugins/scene3d/Scene3D.cc | 65 +++++++++++++++++++--- src/gui/plugins/scene3d/Scene3D.hh | 5 ++ src/gui/plugins/shapes/Shapes.cc | 80 ++++++++++++++++++++++++++- 4 files changed, 142 insertions(+), 11 deletions(-) diff --git a/src/gui/plugins/scene3d/GzScene3D.qml b/src/gui/plugins/scene3d/GzScene3D.qml index 661c0fb57a..aeec5ce7fc 100644 --- a/src/gui/plugins/scene3d/GzScene3D.qml +++ b/src/gui/plugins/scene3d/GzScene3D.qml @@ -37,6 +37,9 @@ Rectangle { anchors.fill: parent hoverEnabled: true acceptedButtons: Qt.NoButton + onEntered: { + GzScene3D.OnFocusWindow() + } } RenderWindow { diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index c7c0f02c81..0dd7917772 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -248,6 +248,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Name of service for setting entity pose public: std::string poseCmdService; + /// \brief Name of service for creating entity + public: std::string createCmdService; + /// \brief The starting world pose of a clicked visual. public: ignition::math::Vector3d startWorldPos = math::Vector3d::Zero; @@ -575,6 +578,7 @@ void IgnRenderer::Render() IGN_PROFILE("IgnRenderer::Render Shapes"); if (this->dataPtr->shapes) { + /* ignwarn << "1\n"; rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); ignwarn << "2\n"; @@ -608,6 +612,7 @@ void IgnRenderer::Render() } } } + */ this->dataPtr->placingModel = true; ignwarn << "7\n"; /* @@ -785,16 +790,23 @@ void IgnRenderer::HandleModelPlacement() { if (this->dataPtr->placingModel) { + /* ignwarn << "Getting render window\n"; auto renderWindow = this->PluginItem()->findChild(); ignwarn << "Forcing render window focus\n"; renderWindow->forceActiveFocus(); + */ + // check for mouse events + //if (!this->dataPtr->mouseDirty) + // return; if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::MOVE) { ignwarn << "moving to " << this->ScreenToScene(this->dataPtr->mouseEvent.Pos()) << "\n"; } if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT) { + if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS) + { // TODO spawn the object with the logic below /* ignition::msgs::Pose req; @@ -809,15 +821,39 @@ void IgnRenderer::HandleModelPlacement() this->dataPtr->node.Request(this->dataPtr->poseCmdService, req, cb); ignwarn << "placing at " << this->ScreenToScene(this->dataPtr->mouseEvent.Pos()) << "\n"; this->dataPtr->placingModel = false; + */ + sdf::Root root; + root.LoadSdfString(this->dataPtr->model); + sdf::Model model = *(root.ModelByIndex(0)); + math::Pose3d modelPose = model.Pose(); + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting pose" << std::endl; + }; + math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); msgs::EntityFactory req; - req.set_sdf_filename(_drop.toStdString()); + req.set_sdf(this->dataPtr->model); req.set_allow_renaming(true); msgs::Set(req.mutable_pose(), - math::Pose3d(pos.X(), pos.Y(), pos.Z(), 1, 0, 0, 0)); + math::Pose3d(pos.X(), pos.Y(), modelPose.Pos().Z(), modelPose.Rot().W(), modelPose.Rot().X(), modelPose.Rot().Y(), modelPose.Rot().Z())); - this->dataPtr->node.Request("/world/" + this->dataPtr->worldName + "/create", - req, cb); - */ + // TODO get the pose from the sdf string to get z coordinate and pose ...? + + if (this->dataPtr->createCmdService.empty()) + { + this->dataPtr->createCmdService = "/world/" + this->worldName + + "/create"; + } + ignwarn << "pose cmd service " << this->dataPtr->createCmdService << "\n"; + ignwarn << "model string " << this->dataPtr->model << "\n"; + ignwarn << "placing at " << pos << "\n"; + this->dataPtr->node.Request(this->dataPtr->createCmdService, req, cb); + this->dataPtr->placingModel = false; + ignwarn << "Placing model\n"; + this->dataPtr->mouseDirty = false; + } } } } @@ -1746,6 +1782,7 @@ RenderWindowItem::RenderWindowItem(QQuickItem *_parent) done = true; this->setAcceptedMouseButtons(Qt::AllButtons); + this->setAcceptHoverEvents(true); this->setFlag(ItemHasContents); this->dataPtr->renderThread = new RenderThread(); } @@ -2190,6 +2227,13 @@ void Scene3D::OnDropped(const QString &_drop, int _mouseX, int _mouseY) req, cb); } +///////////////////////////////////////////////// +void Scene3D::OnFocusWindow() +{ + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->forceActiveFocus(); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetXYZSnap(const math::Vector3d &_xyz) { @@ -2367,10 +2411,15 @@ void RenderWindowItem::SetWorldName(const std::string &_name) } ///////////////////////////////////////////////// -void RenderWindowItem::mousePressEvent(QMouseEvent *_e) +void RenderWindowItem::hoverMoveEvent(QHoverEvent *_e) { - this->forceActiveFocus(); + math::Vector2i pos = {_e->pos().x(), _e->pos().y()}; + ignwarn << "pos " << this->ScreenToScene(pos) << "\n"; +} +///////////////////////////////////////////////// +void RenderWindowItem::mousePressEvent(QMouseEvent *_e) +{ auto event = ignition::gui::convert(*_e); event.SetPressPos(event.Pos()); this->dataPtr->mouseEvent = event; @@ -2419,8 +2468,6 @@ void RenderWindowItem::mouseMoveEvent(QMouseEvent *_e) //////////////////////////////////////////////// void RenderWindowItem::wheelEvent(QWheelEvent *_e) { - this->forceActiveFocus(); - this->dataPtr->mouseEvent.SetType(common::MouseEvent::SCROLL); this->dataPtr->mouseEvent.SetPos(_e->x(), _e->y()); double scroll = (_e->angleDelta().y() > 0) ? -1.0 : 1.0; diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 99d37f9e6c..b74236a052 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -96,6 +96,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public slots: void OnDropped(const QString &_drop, int _mouseX, int _mouseY); + public slots: void OnFocusWindow(); + // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; @@ -531,6 +533,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Slot called when thread is ready to be started public Q_SLOTS: void Ready(); + // Documentation inherited + protected: void hoverMoveEvent(QHoverEvent *_e) override; + // Documentation inherited protected: void mousePressEvent(QMouseEvent *_e) override; diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index 49203bf41c..b9ecdb52e0 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -127,11 +127,87 @@ void Shapes::OnMode(const QString &_mode) } else if (_mode == "sphere") { - + sdfString = std::string("" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + "" + "1" + "0" + "0" + "1" + "0" + "1" + "" + "1.0" + "" + "" + "" + "" + "0.5" + "" + "" + "" + "" + "" + "" + "0.5" + "" + "" + "" + "1 0 0 1" + "1 0 0 1" + "1 0 0 1" + "" + "" + "" + "" + ""); } else if (_mode == "cylinder") { - + sdfString = std::string("" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + "" + "1" + "0" + "0" + "1" + "0" + "1" + "" + "1.0" + "" + "" + "" + "" + "0.5" + "1.0" + "" + "" + "" + "" + "" + "" + "0.5" + "1.0" + "" + "" + "" + "1 0 0 1" + "1 0 0 1" + "1 0 0 1" + "" + "" + "" + "" + ""); } std::string path = std::string(PROJECT_SOURCE_PATH) + "/src/gui/plugins/shapes/" + _mode.toStdString() + ".sdf"; req.set_data(sdfString); From 250a9666344e20487aeba06859f7def320c90243 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Mon, 27 Apr 2020 17:39:12 -0700 Subject: [PATCH 06/18] Fix tracking, add uuid generation Signed-off-by: John Shepherd --- src/gui/plugins/scene3d/Scene3D.cc | 156 ++++++++++++++++++++--------- src/gui/plugins/scene3d/Scene3D.hh | 7 ++ src/gui/plugins/shapes/Shapes.cc | 16 --- 3 files changed, 116 insertions(+), 63 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 0dd7917772..e7cd6c848e 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -139,6 +140,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Flag to indicate if mouse event is dirty public: bool mouseDirty = false; + /// \brief Flag to indicate if hover event is dirty + public: bool hoverDirty = false; + /// \brief Mouse event public: common::MouseEvent mouseEvent; @@ -216,7 +220,19 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// model with the shapes plugin or not public: bool placingModel = false; - public: std::string model; + /// \brief The sdf string of the model to be used with the shapes plugin + public: std::string modelSdfString; + + /// \brief The currently hovered mouse position in screen coordinates + public: math::Vector2i mouseHoverPos = math::Vector2i::Zero; + + /// \brief The model generated from the modelSdfString upon the user + /// clicking a shape in the shapes plugin + public: rendering::VisualPtr model = nullptr; + + /// \brief A record of the ids currently used by the model generation + /// for easy deletion of visuals later + public: std::vector modelIds; /// \brief The pose set during a view angle button press that holds /// the pose the camera should assume relative to the entit(y/ies). @@ -578,7 +594,6 @@ void IgnRenderer::Render() IGN_PROFILE("IgnRenderer::Render Shapes"); if (this->dataPtr->shapes) { - /* ignwarn << "1\n"; rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); ignwarn << "2\n"; @@ -586,7 +601,7 @@ void IgnRenderer::Render() ignwarn << "3\n"; sdf::Root root; - root.LoadSdfString(this->dataPtr->model); + root.LoadSdfString(this->dataPtr->modelSdfString); ignwarn << "4\n"; sdf::Model model2 = *(root.ModelByIndex(0)); ignwarn << "6\n"; @@ -595,24 +610,36 @@ void IgnRenderer::Render() { ignwarn << "5\n"; sdf::Model model = *(root.ModelByIndex(i)); + model.SetName(ignition::common::Uuid().String()); ignwarn << "6\n"; ignwarn << "model name: " << model.Name() << "\n"; - this->dataPtr->renderUtil.SceneManager().CreateModel(1000, model, this->dataPtr->renderUtil.SceneManager().WorldId()); + // TODO generate unique entity id + Entity modelEntityId = this->UniqueId(); + ignwarn << "model entity " << modelEntityId << "\n"; + this->dataPtr->model = this->dataPtr->renderUtil.SceneManager().CreateModel(modelEntityId, model, this->dataPtr->renderUtil.SceneManager().WorldId()); + this->dataPtr->modelIds.push_back(modelEntityId); for (auto j = 0u; j < model.LinkCount(); j++) { sdf::Link link = *(model.LinkByIndex(j)); + link.SetName(ignition::common::Uuid().String()); ignwarn << "link name: " << link.Name() << "\n"; - this->dataPtr->renderUtil.SceneManager().CreateLink(999, link, 1000); + Entity linkId = this->UniqueId(); + ignwarn << "link entity " << linkId << "\n"; + this->dataPtr->renderUtil.SceneManager().CreateLink(linkId, link, modelEntityId); + this->dataPtr->modelIds.push_back(linkId); for (auto k = 0u; k < link.VisualCount(); k++) { sdf::Visual visual = *(link.VisualByIndex(k)); + visual.SetName(ignition::common::Uuid().String()); ignwarn << "visual name: " << visual.Name() << "\n"; ignwarn << "geom " << static_cast(visual.Geom()->Type()) << "\n"; - this->dataPtr->renderUtil.SceneManager().CreateVisual(998, visual, 999); + Entity visualId = this->UniqueId(); + ignwarn << "visual entity " << visualId << "\n"; + this->dataPtr->renderUtil.SceneManager().CreateVisual(visualId, visual, linkId); + this->dataPtr->modelIds.push_back(visualId); } } } - */ this->dataPtr->placingModel = true; ignwarn << "7\n"; /* @@ -637,6 +664,19 @@ void IgnRenderer::Render() } } +///////////////////////////////////////////////// +Entity IgnRenderer::UniqueId() +{ + auto timeout = 100000u; + for (auto i = 0u; i < timeout; ++i) + { + Entity id = std::numeric_limits::max() - i; + if (!this->dataPtr->renderUtil.SceneManager().HasEntity(id)) + return id; + } + return kNullEntity; +} + ///////////////////////////////////////////////// void IgnRenderer::HandleMouseEvent() { @@ -790,40 +830,30 @@ void IgnRenderer::HandleModelPlacement() { if (this->dataPtr->placingModel) { - /* - ignwarn << "Getting render window\n"; - auto renderWindow = this->PluginItem()->findChild(); - ignwarn << "Forcing render window focus\n"; - renderWindow->forceActiveFocus(); - */ - // check for mouse events - //if (!this->dataPtr->mouseDirty) - // return; - if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::MOVE) + ignwarn << "mouse hover pos " << this->dataPtr->mouseHoverPos << "\n"; + if (this->dataPtr->model) { - ignwarn << "moving to " << this->ScreenToScene(this->dataPtr->mouseEvent.Pos()) << "\n"; + if (this->dataPtr->hoverDirty) + { + math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseHoverPos); + double z = this->dataPtr->model->WorldPosition().Z(); + math::Vector3d newPos = {pos.X(), pos.Y(), z}; + this->dataPtr->model->SetWorldPosition(newPos); + this->dataPtr->hoverDirty = false; + } } - if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT) + if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && + this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS) { - if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS) - { - // TODO spawn the object with the logic below - /* - ignition::msgs::Pose req; - req.set_name(node->Name()); - msgs::Set(req.mutable_position(), node->WorldPosition()); - msgs::Set(req.mutable_orientation(), node->WorldRotation()); - if (this->dataPtr->poseCmdService.empty()) + for (auto _id : this->dataPtr->modelIds) { - this->dataPtr->poseCmdService = "/world/" + this->worldName - + "/set_pose"; + ignwarn << "Removing " << _id << "\n"; + this->dataPtr->renderUtil.SceneManager().RemoveEntity(_id); } - this->dataPtr->node.Request(this->dataPtr->poseCmdService, req, cb); - ignwarn << "placing at " << this->ScreenToScene(this->dataPtr->mouseEvent.Pos()) << "\n"; - this->dataPtr->placingModel = false; - */ + this->dataPtr->modelIds.clear(); + sdf::Root root; - root.LoadSdfString(this->dataPtr->model); + root.LoadSdfString(this->dataPtr->modelSdfString); sdf::Model model = *(root.ModelByIndex(0)); math::Pose3d modelPose = model.Pose(); std::function cb = @@ -832,28 +862,21 @@ void IgnRenderer::HandleModelPlacement() if (!_result) ignerr << "Error setting pose" << std::endl; }; - math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); + math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseEvent.Pos()); msgs::EntityFactory req; - req.set_sdf(this->dataPtr->model); + req.set_sdf(this->dataPtr->modelSdfString); req.set_allow_renaming(true); msgs::Set(req.mutable_pose(), math::Pose3d(pos.X(), pos.Y(), modelPose.Pos().Z(), modelPose.Rot().W(), modelPose.Rot().X(), modelPose.Rot().Y(), modelPose.Rot().Z())); - - // TODO get the pose from the sdf string to get z coordinate and pose ...? - + if (this->dataPtr->createCmdService.empty()) { this->dataPtr->createCmdService = "/world/" + this->worldName + "/create"; } - ignwarn << "pose cmd service " << this->dataPtr->createCmdService << "\n"; - ignwarn << "model string " << this->dataPtr->model << "\n"; - ignwarn << "placing at " << pos << "\n"; this->dataPtr->node.Request(this->dataPtr->createCmdService, req, cb); this->dataPtr->placingModel = false; - ignwarn << "Placing model\n"; this->dataPtr->mouseDirty = false; - } } } } @@ -1508,7 +1531,7 @@ void IgnRenderer::SetModel(const std::string &_model) { std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->shapes = true; - this->dataPtr->model = _model; + this->dataPtr->modelSdfString = _model; } ///////////////////////////////////////////////// @@ -1608,6 +1631,14 @@ void IgnRenderer::OnViewAngleComplete() this->dataPtr->viewAngle = false; } +///////////////////////////////////////////////// +void IgnRenderer::NewHoverEvent(const math::Vector2i &_mousePos) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->mouseHoverPos = _mousePos; + this->dataPtr->hoverDirty = true; +} + ///////////////////////////////////////////////// void IgnRenderer::NewMouseEvent(const common::MouseEvent &_e, const math::Vector2d &_drag) @@ -1618,6 +1649,37 @@ void IgnRenderer::NewMouseEvent(const common::MouseEvent &_e, this->dataPtr->mouseDirty = true; } +///////////////////////////////////////////////// +math::Vector3d IgnRenderer::ScreenToPlane( + const math::Vector2i &_screenPos) const +{ + // Normalize point on the image + double width = this->dataPtr->camera->ImageWidth(); + double height = this->dataPtr->camera->ImageHeight(); + + double nx = 2.0 * _screenPos.X() / width - 1.0; + double ny = 1.0 - 2.0 * _screenPos.Y() / height; + + // Make a ray query + this->dataPtr->rayQuery->SetFromCamera( + this->dataPtr->camera, math::Vector2d(nx, ny)); + + auto result = this->dataPtr->rayQuery->ClosestPoint(); + ignition::math::Planed plane(ignition::math::Vector3d(0, 0, 1), 0); + + if (result) + { + math::Vector3d origin = this->dataPtr->rayQuery->Origin(); + math::Vector3d direction = this->dataPtr->rayQuery->Direction(); + double distance = plane.Distance(origin, direction); + return origin + direction * distance; + } + + // Set point to be 10m away if no intersection found + return this->dataPtr->rayQuery->Origin() + + this->dataPtr->rayQuery->Direction() * 10; +} + ///////////////////////////////////////////////// math::Vector3d IgnRenderer::ScreenToScene( const math::Vector2i &_screenPos) const @@ -2414,7 +2476,7 @@ void RenderWindowItem::SetWorldName(const std::string &_name) void RenderWindowItem::hoverMoveEvent(QHoverEvent *_e) { math::Vector2i pos = {_e->pos().x(), _e->pos().y()}; - ignwarn << "pos " << this->ScreenToScene(pos) << "\n"; + this->dataPtr->renderThread->ignRenderer.NewHoverEvent(pos); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index b74236a052..45033d6623 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -244,6 +244,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void NewMouseEvent(const common::MouseEvent &_e, const math::Vector2d &_drag = math::Vector2d::Zero); + public: void NewHoverEvent(const math::Vector2i &_mousePos); + /// \brief Handle key press event for snapping /// \param[in] _e The key event to process. public: void HandleKeyPress(QKeyEvent *_e); @@ -328,6 +330,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Handle model placement requests private: void HandleModelPlacement(); + private: Entity UniqueId(); + + public: math::Vector3d ScreenToPlane(const math::Vector2i &_screenPos) + const; + /// \brief Retrieve the first point on a surface in the 3D scene hit by a /// ray cast from the given 2D screen coordinates. /// \param[in] _screenPos 2D coordinates on the screen, in pixels. diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index b9ecdb52e0..2b41c92c9a 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -115,11 +115,6 @@ void Shapes::OnMode(const QString &_mode) "1 1 1" "" "" - "" - "1 0 0 1" - "1 0 0 1" - "1 0 0 1" - "" "" "" "" @@ -156,11 +151,6 @@ void Shapes::OnMode(const QString &_mode) "0.5" "" "" - "" - "1 0 0 1" - "1 0 0 1" - "1 0 0 1" - "" "" "" "" @@ -199,17 +189,11 @@ void Shapes::OnMode(const QString &_mode) "1.0" "" "" - "" - "1 0 0 1" - "1 0 0 1" - "1 0 0 1" - "" "" "" "" ""); } - std::string path = std::string(PROJECT_SOURCE_PATH) + "/src/gui/plugins/shapes/" + _mode.toStdString() + ".sdf"; req.set_data(sdfString); this->dataPtr->node.Request(this->dataPtr->service, req, cb); } From c2721a7edfb31601f0334c9924380c4f65245fff Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Mon, 27 Apr 2020 18:31:17 -0700 Subject: [PATCH 07/18] Style fixes Signed-off-by: John Shepherd --- .../ignition/gazebo/rendering/SceneManager.hh | 24 +++--- src/gui/plugins/scene3d/Scene3D.cc | 83 +++++++++---------- src/gui/plugins/scene3d/Scene3D.hh | 14 +++- src/gui/plugins/shapes/CMakeLists.txt | 2 - src/rendering/SceneManager.cc | 9 -- 5 files changed, 64 insertions(+), 68 deletions(-) diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 30636933a0..ce2f571ee5 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -52,11 +52,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Destructor public: ~SceneManager(); - /// \brief Set the scene to manage + /// \brief Set the scene to manage. /// \param[in] _scene Scene pointer. public: void SetScene(rendering::ScenePtr _scene); - /// \brief Get the scene + /// \brief Get the scene. /// \return Pointer to scene public: rendering::ScenePtr Scene() const; @@ -65,10 +65,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void SetWorldId(Entity _id); /// \brief Get the world's ID. - /// \return World ID. + /// \return World ID public: Entity WorldId(); - /// \brief Create a model + /// \brief Create a model. /// \param[in] _id Unique model id /// \param[in] _model Model sdf dom /// \param[in] _parentId Parent id @@ -76,7 +76,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateModel(Entity _id, const sdf::Model &_model, Entity _parentId = 0); - /// \brief Create a link + /// \brief Create a link. /// \param[in] _id Unique link id /// \param[in] _link Link sdf dom /// \param[in] _parentId Parent id @@ -84,7 +84,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateLink(Entity _id, const sdf::Link &_link, Entity _parentId = 0); - /// \brief Create a visual + /// \brief Create a visual. /// \param[in] _id Unique visual id /// \param[in] _visual Visual sdf dom /// \param[in] _parentId Parent id @@ -92,7 +92,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateVisual(Entity _id, const sdf::Visual &_visual, Entity _parentId = 0); - /// \brief Create a light + /// \brief Create a light. /// \param[in] _id Unique light id /// \param[in] _light Light sdf dom /// \param[in] _parentId Parent id @@ -110,17 +110,17 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: bool AddSensor(Entity _gazeboId, const std::string &_sensorName, Entity _parentGazeboId = 0); - /// \brief Check if entity exists + /// \brief Check if entity exists. /// \param[in] _id Unique entity id /// \return true if exists, false otherwise public: bool HasEntity(Entity _id) const; - /// \brief Get a rendering node given an entity id + /// \brief Get a rendering node given an entity id. /// \param[in] _id Entity's unique id /// \return Pointer to requested entity's node public: rendering::NodePtr NodeById(Entity _id) const; - /// \brief Remove an entity by id + /// \brief Remove an entity by id. /// \param[in] _id Entity's unique id public: void RemoveEntity(Entity _id); @@ -129,7 +129,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return The entity for that node, or `kNullEntity` for no entity. public: Entity EntityFromNode(const rendering::NodePtr &_node) const; - /// \brief Load a geometry + /// \brief Load a geometry. /// \param[in] _geom Geometry sdf dom /// \param[out] _scale Geometry scale that will be set based on sdf /// \param[out] _localPose Additional local pose to be applied after the @@ -138,7 +138,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: rendering::GeometryPtr LoadGeometry(const sdf::Geometry &_geom, math::Vector3d &_scale, math::Pose3d &_localPose); - /// \brief Load a material + /// \brief Load a material. /// \param[in] _material Material sdf dom /// \return Material object loaded from the sdf dom private: rendering::MaterialPtr LoadMaterial( diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index e7cd6c848e..5107075693 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -589,69 +589,61 @@ void IgnRenderer::Render() } // Shapes - // TODO move shapes functionality here to reside within render thread { IGN_PROFILE("IgnRenderer::Render Shapes"); if (this->dataPtr->shapes) { - ignwarn << "1\n"; rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); - ignwarn << "2\n"; rendering::VisualPtr rootVis = scene->RootVisual(); - ignwarn << "3\n"; sdf::Root root; root.LoadSdfString(this->dataPtr->modelSdfString); - ignwarn << "4\n"; - sdf::Model model2 = *(root.ModelByIndex(0)); - ignwarn << "6\n"; - ignwarn << "model name: " << model2.Name() << "\n"; for (auto i = 0u; i < root.ModelCount(); i++) { - ignwarn << "5\n"; sdf::Model model = *(root.ModelByIndex(i)); model.SetName(ignition::common::Uuid().String()); - ignwarn << "6\n"; - ignwarn << "model name: " << model.Name() << "\n"; - // TODO generate unique entity id - Entity modelEntityId = this->UniqueId(); - ignwarn << "model entity " << modelEntityId << "\n"; - this->dataPtr->model = this->dataPtr->renderUtil.SceneManager().CreateModel(modelEntityId, model, this->dataPtr->renderUtil.SceneManager().WorldId()); + Entity modelId = this->UniqueId(); + if (!modelId) + { + this->DeleteVisualModel(); + break; + } + this->dataPtr->model = + this->dataPtr->renderUtil.SceneManager().CreateModel( + modelId, model, + this->dataPtr->renderUtil.SceneManager().WorldId()); + this->dataPtr->modelIds.push_back(modelEntityId); for (auto j = 0u; j < model.LinkCount(); j++) { sdf::Link link = *(model.LinkByIndex(j)); link.SetName(ignition::common::Uuid().String()); - ignwarn << "link name: " << link.Name() << "\n"; Entity linkId = this->UniqueId(); - ignwarn << "link entity " << linkId << "\n"; - this->dataPtr->renderUtil.SceneManager().CreateLink(linkId, link, modelEntityId); + if (!linkId) + { + this->DeleteVisualModel(); + break; + } + this->dataPtr->renderUtil.SceneManager().CreateLink( + linkId, link, modelId); this->dataPtr->modelIds.push_back(linkId); for (auto k = 0u; k < link.VisualCount(); k++) { sdf::Visual visual = *(link.VisualByIndex(k)); visual.SetName(ignition::common::Uuid().String()); - ignwarn << "visual name: " << visual.Name() << "\n"; - ignwarn << "geom " << static_cast(visual.Geom()->Type()) << "\n"; Entity visualId = this->UniqueId(); - ignwarn << "visual entity " << visualId << "\n"; - this->dataPtr->renderUtil.SceneManager().CreateVisual(visualId, visual, linkId); + if (!visualId) + { + this->DeleteVisualModel(); + break; + } + this->dataPtr->renderUtil.SceneManager().CreateVisual( + visualId, visual, linkId); this->dataPtr->modelIds.push_back(visualId); } } } this->dataPtr->placingModel = true; - ignwarn << "7\n"; - /* - msgs::EntityFactory req; - req.set_sdf_filename(_drop.toStdString()); - req.set_allow_renaming(true); - msgs::Set(req.mutable_pose(), - math::Pose3d(pos.X(), pos.Y(), pos.Z(), 1, 0, 0, 0)); - - this->dataPtr->node.Request("/world/" + this->dataPtr->worldName + "/create", - req, cb); - */ this->dataPtr->shapes = false; } } @@ -664,6 +656,14 @@ void IgnRenderer::Render() } } +///////////////////////////////////////////////// +void IgnRenderer::DeleteVisualModel() +{ + for (auto _id : this->dataPtr->modelIds) + this->dataPtr->renderUtil.SceneManager().RemoveEntity(_id); + this->dataPtr->modelIds.clear(); +} + ///////////////////////////////////////////////// Entity IgnRenderer::UniqueId() { @@ -830,7 +830,6 @@ void IgnRenderer::HandleModelPlacement() { if (this->dataPtr->placingModel) { - ignwarn << "mouse hover pos " << this->dataPtr->mouseHoverPos << "\n"; if (this->dataPtr->model) { if (this->dataPtr->hoverDirty) @@ -845,12 +844,8 @@ void IgnRenderer::HandleModelPlacement() if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS) { - for (auto _id : this->dataPtr->modelIds) - { - ignwarn << "Removing " << _id << "\n"; - this->dataPtr->renderUtil.SceneManager().RemoveEntity(_id); - } - this->dataPtr->modelIds.clear(); + // Delete the generated visuals + this->DeleteVisualModel() sdf::Root root; root.LoadSdfString(this->dataPtr->modelSdfString); @@ -863,12 +858,12 @@ void IgnRenderer::HandleModelPlacement() ignerr << "Error setting pose" << std::endl; }; math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseEvent.Pos()); + pos.Z(modelPose.Pos().Z()); msgs::EntityFactory req; req.set_sdf(this->dataPtr->modelSdfString); req.set_allow_renaming(true); - msgs::Set(req.mutable_pose(), - math::Pose3d(pos.X(), pos.Y(), modelPose.Pos().Z(), modelPose.Rot().W(), modelPose.Rot().X(), modelPose.Rot().Y(), modelPose.Rot().Z())); - + msgs::Set(req.mutable_pose(), math::Pose3d(pos, modelPose.Rot())); + if (this->dataPtr->createCmdService.empty()) { this->dataPtr->createCmdService = "/world/" + this->worldName @@ -1666,7 +1661,7 @@ math::Vector3d IgnRenderer::ScreenToPlane( auto result = this->dataPtr->rayQuery->ClosestPoint(); ignition::math::Planed plane(ignition::math::Vector3d(0, 0, 1), 0); - + if (result) { math::Vector3d origin = this->dataPtr->rayQuery->Origin(); diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 45033d6623..c3567e0813 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -96,6 +96,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public slots: void OnDropped(const QString &_drop, int _mouseX, int _mouseY); + /// \brief Callback when a focus event is received. public slots: void OnFocusWindow(); // Documentation inherited @@ -180,7 +181,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _mode New transform mode to set to public: void SetTransformMode(const std::string &_mode); - /// \brief Set the model to hover. + /// \brief Set the model to hover over the scene. /// \param[in] _mode Path to new model to load in for the user. public: void SetModel(const std::string &_model); @@ -244,6 +245,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void NewMouseEvent(const common::MouseEvent &_e, const math::Vector2d &_drag = math::Vector2d::Zero); + /// \brief New hover event triggered. + /// \param[in] _mousePos Mouse hover screen position public: void NewHoverEvent(const math::Vector2i &_mousePos); /// \brief Handle key press event for snapping @@ -330,8 +333,17 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Handle model placement requests private: void HandleModelPlacement(); + /// \brief Generate a unique entity id. + /// \return The unique entity id private: Entity UniqueId(); + /// \brief Delete the visuals generated from the shapes plugin. + private: void DeleteVisualModel(); + + /// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a + /// ray cast from the given 2D screen coordinates. + /// \param[in] _screenPod 2D coordinates on the screen, in pixels. + /// \return 3D coordinates of a point in the 3D scene. public: math::Vector3d ScreenToPlane(const math::Vector2i &_screenPos) const; diff --git a/src/gui/plugins/shapes/CMakeLists.txt b/src/gui/plugins/shapes/CMakeLists.txt index 9542555733..fd6c2d7458 100644 --- a/src/gui/plugins/shapes/CMakeLists.txt +++ b/src/gui/plugins/shapes/CMakeLists.txt @@ -2,5 +2,3 @@ gz_add_gui_plugin(Shapes SOURCES Shapes.cc QT_HEADERS Shapes.hh ) - - diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index a8591894b7..5b4cc5af0e 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -187,14 +187,12 @@ rendering::VisualPtr SceneManager::CreateLink(Entity _id, rendering::VisualPtr SceneManager::CreateVisual(Entity _id, const sdf::Visual &_visual, Entity _parentId) { - ignwarn << "1\n"; if (this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end()) { ignerr << "Entity with Id: [" << _id << "] already exists in the scene" << std::endl; return rendering::VisualPtr(); } - ignwarn << "2\n"; rendering::VisualPtr parent; if (_parentId != this->dataPtr->worldId) @@ -208,7 +206,6 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, } parent = it->second; } - ignwarn << "3\n"; if (!_visual.Geom()) return rendering::VisualPtr(); @@ -219,17 +216,14 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, name = parent->Name() + "::" + name; rendering::VisualPtr visualVis = this->dataPtr->scene->CreateVisual(name); visualVis->SetLocalPose(_visual.Pose()); - ignwarn << "4\n"; math::Vector3d scale = math::Vector3d::One; math::Pose3d localPose; rendering::GeometryPtr geom = this->LoadGeometry(*_visual.Geom(), scale, localPose); - ignwarn << "5\n"; if (geom) { - ignwarn << "6\n"; /// localPose is currently used to handle the normal vector in plane visuals /// In general, this can be used to store any local transforms between the /// parent Visual and geometry. @@ -269,7 +263,6 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, } else { - ignwarn << "7\n"; // meshes created by mesh loader may have their own materials // update/override their properties based on input sdf element values auto mesh = std::dynamic_pointer_cast(geom); @@ -325,9 +318,7 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, rendering::GeometryPtr geom{nullptr}; if (_geom.Type() == sdf::GeometryType::BOX) { - ignwarn << "Creating box\n"; geom = this->dataPtr->scene->CreateBox(); - ignwarn << "Created box\n"; scale = _geom.BoxShape()->Size(); } else if (_geom.Type() == sdf::GeometryType::CYLINDER) From e84f8c939cfff0374c924eaa7294061234269c50 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Mon, 27 Apr 2020 18:59:18 -0700 Subject: [PATCH 08/18] Add escape exit for shapes Signed-off-by: John Shepherd --- .../ignition/gazebo/rendering/SceneManager.hh | 20 ++++++++--------- src/gui/plugins/scene3d/Scene3D.cc | 22 +++++++++---------- src/gui/plugins/scene3d/Scene3D.hh | 2 +- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index ce2f571ee5..4c6fd29480 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -52,11 +52,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Destructor public: ~SceneManager(); - /// \brief Set the scene to manage. + /// \brief Set the scene to manage /// \param[in] _scene Scene pointer. public: void SetScene(rendering::ScenePtr _scene); - /// \brief Get the scene. + /// \brief Get the scene /// \return Pointer to scene public: rendering::ScenePtr Scene() const; @@ -76,7 +76,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateModel(Entity _id, const sdf::Model &_model, Entity _parentId = 0); - /// \brief Create a link. + /// \brief Create a link /// \param[in] _id Unique link id /// \param[in] _link Link sdf dom /// \param[in] _parentId Parent id @@ -84,7 +84,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateLink(Entity _id, const sdf::Link &_link, Entity _parentId = 0); - /// \brief Create a visual. + /// \brief Create a visual /// \param[in] _id Unique visual id /// \param[in] _visual Visual sdf dom /// \param[in] _parentId Parent id @@ -92,7 +92,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateVisual(Entity _id, const sdf::Visual &_visual, Entity _parentId = 0); - /// \brief Create a light. + /// \brief Create a light /// \param[in] _id Unique light id /// \param[in] _light Light sdf dom /// \param[in] _parentId Parent id @@ -110,17 +110,17 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: bool AddSensor(Entity _gazeboId, const std::string &_sensorName, Entity _parentGazeboId = 0); - /// \brief Check if entity exists. + /// \brief Check if entity exists /// \param[in] _id Unique entity id /// \return true if exists, false otherwise public: bool HasEntity(Entity _id) const; - /// \brief Get a rendering node given an entity id. + /// \brief Get a rendering node given an entity id /// \param[in] _id Entity's unique id /// \return Pointer to requested entity's node public: rendering::NodePtr NodeById(Entity _id) const; - /// \brief Remove an entity by id. + /// \brief Remove an entity by id /// \param[in] _id Entity's unique id public: void RemoveEntity(Entity _id); @@ -129,7 +129,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return The entity for that node, or `kNullEntity` for no entity. public: Entity EntityFromNode(const rendering::NodePtr &_node) const; - /// \brief Load a geometry. + /// \brief Load a geometry /// \param[in] _geom Geometry sdf dom /// \param[out] _scale Geometry scale that will be set based on sdf /// \param[out] _localPose Additional local pose to be applied after the @@ -138,7 +138,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: rendering::GeometryPtr LoadGeometry(const sdf::Geometry &_geom, math::Vector3d &_scale, math::Pose3d &_localPose); - /// \brief Load a material. + /// \brief Load a material /// \param[in] _material Material sdf dom /// \return Material object loaded from the sdf dom private: rendering::MaterialPtr LoadMaterial( diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 5107075693..0bb0371c55 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -23,6 +23,8 @@ #include #include +#include +#include #include #include @@ -613,7 +615,7 @@ void IgnRenderer::Render() modelId, model, this->dataPtr->renderUtil.SceneManager().WorldId()); - this->dataPtr->modelIds.push_back(modelEntityId); + this->dataPtr->modelIds.push_back(modelId); for (auto j = 0u; j < model.LinkCount(); j++) { sdf::Link link = *(model.LinkByIndex(j)); @@ -662,6 +664,7 @@ void IgnRenderer::DeleteVisualModel() for (auto _id : this->dataPtr->modelIds) this->dataPtr->renderUtil.SceneManager().RemoveEntity(_id); this->dataPtr->modelIds.clear(); + this->dataPtr->placingModel = false; } ///////////////////////////////////////////////// @@ -830,22 +833,18 @@ void IgnRenderer::HandleModelPlacement() { if (this->dataPtr->placingModel) { - if (this->dataPtr->model) + if (this->dataPtr->model && this->dataPtr->hoverDirty) { - if (this->dataPtr->hoverDirty) - { - math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseHoverPos); - double z = this->dataPtr->model->WorldPosition().Z(); - math::Vector3d newPos = {pos.X(), pos.Y(), z}; - this->dataPtr->model->SetWorldPosition(newPos); - this->dataPtr->hoverDirty = false; - } + math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseHoverPos); + pos.Z(this->dataPtr->model->WorldPosition().Z()); + this->dataPtr->model->SetWorldPosition(pos); + this->dataPtr->hoverDirty = false; } if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS) { // Delete the generated visuals - this->DeleteVisualModel() + this->DeleteVisualModel(); sdf::Root root; root.LoadSdfString(this->dataPtr->modelSdfString); @@ -2553,6 +2552,7 @@ void RenderWindowItem::keyReleaseEvent(QKeyEvent *_e) _e->accept(); } this->DeselectAllEntities(true); + this->dataPtr->renderThread->ignRenderer.DeleteVisualModel(); } } diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index c3567e0813..c95e7f4b10 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -338,7 +338,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: Entity UniqueId(); /// \brief Delete the visuals generated from the shapes plugin. - private: void DeleteVisualModel(); + public: void DeleteVisualModel(); /// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a /// ray cast from the given 2D screen coordinates. From 3c5913cc3521192bee8e65fa40db479e7c07c777 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Mon, 27 Apr 2020 22:11:37 -0700 Subject: [PATCH 09/18] minor revisions Signed-off-by: John Shepherd --- include/ignition/gazebo/rendering/SceneManager.hh | 2 +- src/gui/plugins/shapes/Shapes.cc | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 4c6fd29480..65e9e663f7 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -68,7 +68,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return World ID public: Entity WorldId(); - /// \brief Create a model. + /// \brief Create a model /// \param[in] _id Unique model id /// \param[in] _model Model sdf dom /// \param[in] _parentId Parent id diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index 2b41c92c9a..7937a3c885 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -25,9 +25,6 @@ #include #include -#include "ignition/gazebo/test_config.hh" -#include "ignition/gazebo/components/Name.hh" -#include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" From 0c4e7766b5f5699072fed6b54533fb9cc675363f Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Tue, 28 Apr 2020 00:23:27 -0700 Subject: [PATCH 10/18] Update placement logic, move handle model function Signed-off-by: John Shepherd --- src/gui/plugins/scene3d/Scene3D.cc | 9 ++++----- src/gui/plugins/shapes/Shapes.cc | 17 +++++++++++++++++ 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 0bb0371c55..3dd8ac8957 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -407,9 +407,6 @@ void IgnRenderer::Render() // Entity selection this->HandleEntitySelection(); - // Model placement - this->HandleModelPlacement(); - // reset follow mode if target node got removed if (!this->dataPtr->followTarget.empty()) { @@ -685,6 +682,7 @@ void IgnRenderer::HandleMouseEvent() { std::lock_guard lock(this->dataPtr->mutex); this->HandleMouseContextMenu(); + this->HandleModelPlacement(); this->HandleMouseTransformControl(); this->HandleMouseViewControl(); } @@ -841,7 +839,8 @@ void IgnRenderer::HandleModelPlacement() this->dataPtr->hoverDirty = false; } if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && - this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS) + this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE && + !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty) { // Delete the generated visuals this->DeleteVisualModel(); @@ -854,7 +853,7 @@ void IgnRenderer::HandleModelPlacement() [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) - ignerr << "Error setting pose" << std::endl; + ignerr << "Error creating model" << std::endl; }; math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseEvent.Pos()); pos.Z(modelPose.Pos().Z()); diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index 7937a3c885..8c2bed93e7 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -112,6 +112,12 @@ void Shapes::OnMode(const QString &_mode) "1 1 1" "" "" + "" + "0.2 0.2 0.2 1" + "0.2 0.2 0.2 1" + "0.2 0.2 0.2 1" + "0 0 0 1" + "" "" "" "" @@ -148,6 +154,12 @@ void Shapes::OnMode(const QString &_mode) "0.5" "" "" + "" + "0.2 0.2 0.2 1" + "0.2 0.2 0.2 1" + "0.2 0.2 0.2 1" + "0 0 0 1" + "" "" "" "" @@ -186,6 +198,11 @@ void Shapes::OnMode(const QString &_mode) "1.0" "" "" + "" + "0.2 0.2 0.2 1" + "0.2 0.2 0.2 1" + "0.2 0.2 0.2 1" + "" "" "" "" From c639e90a39f131f5eb0a4e11c411f3f22368fe45 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Tue, 28 Apr 2020 13:00:12 -0700 Subject: [PATCH 11/18] add force focus back Signed-off-by: John Shepherd --- src/gui/plugins/scene3d/Scene3D.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 3dd8ac8957..e27fba2c50 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -2469,12 +2469,17 @@ void RenderWindowItem::SetWorldName(const std::string &_name) void RenderWindowItem::hoverMoveEvent(QHoverEvent *_e) { math::Vector2i pos = {_e->pos().x(), _e->pos().y()}; + math::Vector2i oldPos = {_e->oldPos().x(), _e->oldPos().y()}; + if (pos == oldPos) + return; this->dataPtr->renderThread->ignRenderer.NewHoverEvent(pos); } ///////////////////////////////////////////////// void RenderWindowItem::mousePressEvent(QMouseEvent *_e) { + this->forceActiveFocus(); + auto event = ignition::gui::convert(*_e); event.SetPressPos(event.Pos()); this->dataPtr->mouseEvent = event; @@ -2523,6 +2528,8 @@ void RenderWindowItem::mouseMoveEvent(QMouseEvent *_e) //////////////////////////////////////////////// void RenderWindowItem::wheelEvent(QWheelEvent *_e) { + this->forceActiveFocus(); + this->dataPtr->mouseEvent.SetType(common::MouseEvent::SCROLL); this->dataPtr->mouseEvent.SetPos(_e->x(), _e->y()); double scroll = (_e->angleDelta().y() > 0) ? -1.0 : 1.0; From 207a9b0fffb40b28bb381e2243409cd6a90e9f37 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Tue, 5 May 2020 11:35:16 -0700 Subject: [PATCH 12/18] Feedback Signed-off-by: John Shepherd --- src/gui/plugins/scene3d/Scene3D.cc | 104 ++++++++++++++--------------- src/gui/plugins/scene3d/Scene3D.hh | 6 +- src/gui/plugins/shapes/Shapes.cc | 35 +++------- src/gui/plugins/shapes/Shapes.hh | 3 - src/gui/plugins/shapes/Shapes.qml | 24 ------- 5 files changed, 64 insertions(+), 108 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index e27fba2c50..7e657c9bc4 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -21,9 +21,9 @@ #include #include -#include -#include #include +#include +#include #include #include @@ -163,7 +163,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Camera orbit controller public: rendering::OrbitViewController viewControl; - /// \brief Transform controller for models + /// \brief Transform controller for public: rendering::TransformController transformControl; /// \brief Transform space: local or world @@ -216,7 +216,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: bool viewAngle = false; /// \brief Flag for indicating whether we are in shapes mode or not - public: bool shapes = false; + public: bool spawnModelMode = false; /// \brief Flag for indicating whether the user is currently placing a /// model with the shapes plugin or not @@ -230,7 +230,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief The model generated from the modelSdfString upon the user /// clicking a shape in the shapes plugin - public: rendering::VisualPtr model = nullptr; + public: rendering::VisualPtr spawnPreviewModel = nullptr; /// \brief A record of the ids currently used by the model generation /// for easy deletion of visuals later @@ -590,7 +590,7 @@ void IgnRenderer::Render() // Shapes { IGN_PROFILE("IgnRenderer::Render Shapes"); - if (this->dataPtr->shapes) + if (this->dataPtr->spawnModelMode) { rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); rendering::VisualPtr rootVis = scene->RootVisual(); @@ -604,10 +604,10 @@ void IgnRenderer::Render() Entity modelId = this->UniqueId(); if (!modelId) { - this->DeleteVisualModel(); + this->TerminateSpawnPreviewModel(); break; } - this->dataPtr->model = + this->dataPtr->spawnPreviewModel = this->dataPtr->renderUtil.SceneManager().CreateModel( modelId, model, this->dataPtr->renderUtil.SceneManager().WorldId()); @@ -620,7 +620,7 @@ void IgnRenderer::Render() Entity linkId = this->UniqueId(); if (!linkId) { - this->DeleteVisualModel(); + this->TerminateSpawnPreviewModel(); break; } this->dataPtr->renderUtil.SceneManager().CreateLink( @@ -633,7 +633,7 @@ void IgnRenderer::Render() Entity visualId = this->UniqueId(); if (!visualId) { - this->DeleteVisualModel(); + this->TerminateSpawnPreviewModel(); break; } this->dataPtr->renderUtil.SceneManager().CreateVisual( @@ -643,7 +643,7 @@ void IgnRenderer::Render() } } this->dataPtr->placingModel = true; - this->dataPtr->shapes = false; + this->dataPtr->spawnModelMode = false; } } @@ -656,7 +656,7 @@ void IgnRenderer::Render() } ///////////////////////////////////////////////// -void IgnRenderer::DeleteVisualModel() +void IgnRenderer::TerminateSpawnPreviewModel() { for (auto _id : this->dataPtr->modelIds) this->dataPtr->renderUtil.SceneManager().RemoveEntity(_id); @@ -829,48 +829,48 @@ void IgnRenderer::HandleKeyRelease(QKeyEvent *_e) ///////////////////////////////////////////////// void IgnRenderer::HandleModelPlacement() { - if (this->dataPtr->placingModel) + if (!this->dataPtr->placingModel) + return; + + if (this->dataPtr->spawnPreviewModel && this->dataPtr->hoverDirty) + { + math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseHoverPos); + pos.Z(this->dataPtr->spawnPreviewModel->WorldPosition().Z()); + this->dataPtr->spawnPreviewModel->SetWorldPosition(pos); + this->dataPtr->hoverDirty = false; + } + if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && + this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE && + !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty) { - if (this->dataPtr->model && this->dataPtr->hoverDirty) + // Delete the generated visuals + this->TerminateSpawnPreviewModel(); + + sdf::Root root; + root.LoadSdfString(this->dataPtr->modelSdfString); + sdf::Model model = *(root.ModelByIndex(0)); + math::Pose3d modelPose = model.Pose(); + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) { - math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseHoverPos); - pos.Z(this->dataPtr->model->WorldPosition().Z()); - this->dataPtr->model->SetWorldPosition(pos); - this->dataPtr->hoverDirty = false; - } - if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && - this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE && - !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty) + if (!_result) + ignerr << "Error creating model" << std::endl; + }; + math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseEvent.Pos()); + pos.Z(modelPose.Pos().Z()); + msgs::EntityFactory req; + req.set_sdf(this->dataPtr->modelSdfString); + req.set_allow_renaming(true); + msgs::Set(req.mutable_pose(), math::Pose3d(pos, modelPose.Rot())); + + if (this->dataPtr->createCmdService.empty()) { - // Delete the generated visuals - this->DeleteVisualModel(); - - sdf::Root root; - root.LoadSdfString(this->dataPtr->modelSdfString); - sdf::Model model = *(root.ModelByIndex(0)); - math::Pose3d modelPose = model.Pose(); - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - ignerr << "Error creating model" << std::endl; - }; - math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseEvent.Pos()); - pos.Z(modelPose.Pos().Z()); - msgs::EntityFactory req; - req.set_sdf(this->dataPtr->modelSdfString); - req.set_allow_renaming(true); - msgs::Set(req.mutable_pose(), math::Pose3d(pos, modelPose.Rot())); - - if (this->dataPtr->createCmdService.empty()) - { - this->dataPtr->createCmdService = "/world/" + this->worldName - + "/create"; - } - this->dataPtr->node.Request(this->dataPtr->createCmdService, req, cb); - this->dataPtr->placingModel = false; - this->dataPtr->mouseDirty = false; + this->dataPtr->createCmdService = "/world/" + this->worldName + + "/create"; } + this->dataPtr->node.Request(this->dataPtr->createCmdService, req, cb); + this->dataPtr->placingModel = false; + this->dataPtr->mouseDirty = false; } } @@ -1523,7 +1523,7 @@ void IgnRenderer::SetTransformMode(const std::string &_mode) void IgnRenderer::SetModel(const std::string &_model) { std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->shapes = true; + this->dataPtr->spawnModelMode = true; this->dataPtr->modelSdfString = _model; } @@ -2558,7 +2558,7 @@ void RenderWindowItem::keyReleaseEvent(QKeyEvent *_e) _e->accept(); } this->DeselectAllEntities(true); - this->dataPtr->renderThread->ignRenderer.DeleteVisualModel(); + this->dataPtr->renderThread->ignRenderer.TerminateSpawnPreviewModel(); } } diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index c95e7f4b10..3087e3b467 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -182,7 +182,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void SetTransformMode(const std::string &_mode); /// \brief Set the model to hover over the scene. - /// \param[in] _mode Path to new model to load in for the user. + /// \param[in] _model Sdf string of the model to load in for the user. public: void SetModel(const std::string &_model); /// \brief Set whether to record video @@ -338,7 +338,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: Entity UniqueId(); /// \brief Delete the visuals generated from the shapes plugin. - public: void DeleteVisualModel(); + public: void TerminateSpawnPreviewModel(); /// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a /// ray cast from the given 2D screen coordinates. @@ -467,7 +467,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void SetTransformMode(const std::string &_mode); /// \brief Set the model to hover. - /// \param[in] _mode Path to new model to load in for the user. + /// \param[in] _model Sdf string of the model to load in for the user. public: void SetModel(const std::string &_model); /// \brief Set whether to record video diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index 8c2bed93e7..c5d54c3111 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -90,12 +90,12 @@ void Shapes::OnMode(const QString &_mode) "" "" "" - "1" + "0.167" "0" "0" - "1" + "0.167" "0" - "1" + "0.167" "" "1.0" "" @@ -112,12 +112,6 @@ void Shapes::OnMode(const QString &_mode) "1 1 1" "" "" - "" - "0.2 0.2 0.2 1" - "0.2 0.2 0.2 1" - "0.2 0.2 0.2 1" - "0 0 0 1" - "" "" "" "" @@ -132,12 +126,12 @@ void Shapes::OnMode(const QString &_mode) "" "" "" - "1" + "0.1" "0" "0" - "1" + "0.1" "0" - "1" + "0.1" "" "1.0" "" @@ -154,12 +148,6 @@ void Shapes::OnMode(const QString &_mode) "0.5" "" "" - "" - "0.2 0.2 0.2 1" - "0.2 0.2 0.2 1" - "0.2 0.2 0.2 1" - "0 0 0 1" - "" "" "" "" @@ -174,12 +162,12 @@ void Shapes::OnMode(const QString &_mode) "" "" "" - "1" + "0.146" "0" "0" - "1" + "0.146" "0" - "1" + "0.125" "" "1.0" "" @@ -198,11 +186,6 @@ void Shapes::OnMode(const QString &_mode) "1.0" "" "" - "" - "0.2 0.2 0.2 1" - "0.2 0.2 0.2 1" - "0.2 0.2 0.2 1" - "" "" "" "" diff --git a/src/gui/plugins/shapes/Shapes.hh b/src/gui/plugins/shapes/Shapes.hh index c3dea2956d..3b8e75216b 100644 --- a/src/gui/plugins/shapes/Shapes.hh +++ b/src/gui/plugins/shapes/Shapes.hh @@ -30,9 +30,6 @@ namespace gazebo /// \brief Provides buttons for adding a box, sphere, or cylinder /// to the scene - /// - /// ## Configuration - /// \ : Set the service to receive transform mode requests. class Shapes : public ignition::gui::Plugin { Q_OBJECT diff --git a/src/gui/plugins/shapes/Shapes.qml b/src/gui/plugins/shapes/Shapes.qml index c2cf537e04..70819c9cf4 100644 --- a/src/gui/plugins/shapes/Shapes.qml +++ b/src/gui/plugins/shapes/Shapes.qml @@ -27,36 +27,14 @@ ToolBar { Layout.minimumWidth: 200 Layout.minimumHeight: 100 - property color snapTitle: (Material.theme == Material.Light) ? - Material.color(Material.Grey, Material.Shade900) : - Material.color(Material.Grey, Material.Shade200) - - property color snapItem: (Material.theme == Material.Light) ? - Material.color(Material.Grey, Material.Shade800) : - Material.color(Material.Grey, Material.Shade100) - - function windowWidth() { - return transformControl.Window.window ? (transformControl.Window.window.width) : 0 - } - - function windowHeight() { - return transformControl.Window.window ? (transformControl.Window.window.height) : 0 - } - background: Rectangle { color: "transparent" } - ButtonGroup { - id: group - } - RowLayout { spacing: 2 ToolButton { id: box - checkable: true - checked: true ButtonGroup.group: group ToolTip.text: "Box" ToolTip.visible: hovered @@ -75,7 +53,6 @@ ToolBar { } ToolButton{ id: sphere - checkable: true ButtonGroup.group: group ToolTip.text: "Sphere" ToolTip.visible: hovered @@ -94,7 +71,6 @@ ToolBar { } ToolButton { id: cylinder - checkable: true ButtonGroup.group: group ToolTip.text: "Cylinder" ToolTip.visible: hovered From dd864dd39ce7b4c87e91c13ec2a390955209d139 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Tue, 5 May 2020 16:15:03 -0700 Subject: [PATCH 13/18] more fixes Signed-off-by: John Shepherd --- include/ignition/gazebo/gui/GuiEvents.hh | 22 +++ src/gui/plugins/scene3d/Scene3D.cc | 158 +++++++-------- src/gui/plugins/scene3d/Scene3D.hh | 14 +- src/gui/plugins/shapes/Shapes.cc | 241 ++++++++++++----------- 4 files changed, 226 insertions(+), 209 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index da28caa77f..524805dd89 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_GUI_GUIEVENTS_HH_ #include +#include #include #include #include @@ -159,6 +160,27 @@ namespace events /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 3); }; + + /// \brief Event called to spawn a preview model. Used in shapes plugin. + class SpawnPreviewModel : public QEvent + { + public: explicit SpawnPreviewModel(std::string &_modelSdfString) + : QEvent(kType), modelSdfString(_modelSdfString) + { + } + /// \brief Unique type for this event. + static const QEvent::Type kType = QEvent::Type(QEvent::User + 4); + + /// \brief Get the sdf string of the model. + /// \return The model sdf string + public: std::string ModelSdfString() const + { + return this->modelSdfString; + } + + /// \brief The sdf string of the model to be previewed. + std::string modelSdfString; + }; } // namespace events } } // namespace gui diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 7e657c9bc4..31e2849a55 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -163,7 +163,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Camera orbit controller public: rendering::OrbitViewController viewControl; - /// \brief Transform controller for + /// \brief Transform controller for models public: rendering::TransformController transformControl; /// \brief Transform space: local or world @@ -594,55 +594,8 @@ void IgnRenderer::Render() { rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); rendering::VisualPtr rootVis = scene->RootVisual(); - - sdf::Root root; - root.LoadSdfString(this->dataPtr->modelSdfString); - for (auto i = 0u; i < root.ModelCount(); i++) - { - sdf::Model model = *(root.ModelByIndex(i)); - model.SetName(ignition::common::Uuid().String()); - Entity modelId = this->UniqueId(); - if (!modelId) - { - this->TerminateSpawnPreviewModel(); - break; - } - this->dataPtr->spawnPreviewModel = - this->dataPtr->renderUtil.SceneManager().CreateModel( - modelId, model, - this->dataPtr->renderUtil.SceneManager().WorldId()); - - this->dataPtr->modelIds.push_back(modelId); - for (auto j = 0u; j < model.LinkCount(); j++) - { - sdf::Link link = *(model.LinkByIndex(j)); - link.SetName(ignition::common::Uuid().String()); - Entity linkId = this->UniqueId(); - if (!linkId) - { - this->TerminateSpawnPreviewModel(); - break; - } - this->dataPtr->renderUtil.SceneManager().CreateLink( - linkId, link, modelId); - this->dataPtr->modelIds.push_back(linkId); - for (auto k = 0u; k < link.VisualCount(); k++) - { - sdf::Visual visual = *(link.VisualByIndex(k)); - visual.SetName(ignition::common::Uuid().String()); - Entity visualId = this->UniqueId(); - if (!visualId) - { - this->TerminateSpawnPreviewModel(); - break; - } - this->dataPtr->renderUtil.SceneManager().CreateVisual( - visualId, visual, linkId); - this->dataPtr->modelIds.push_back(visualId); - } - } - } - this->dataPtr->placingModel = true; + this->dataPtr->placingModel = + this->GeneratePreviewModel(this->dataPtr->modelSdfString); this->dataPtr->spawnModelMode = false; } } @@ -656,7 +609,60 @@ void IgnRenderer::Render() } ///////////////////////////////////////////////// -void IgnRenderer::TerminateSpawnPreviewModel() +bool IgnRenderer::GeneratePreviewModel(const std::string &_modelSdfString) +{ + sdf::Root root; + root.LoadSdfString(_modelSdfString); + for (auto i = 0u; i < root.ModelCount(); i++) + { + sdf::Model model = *(root.ModelByIndex(i)); + model.SetName(ignition::common::Uuid().String()); + Entity modelId = this->UniqueId(); + if (!modelId) + { + this->TerminatePreviewModel(); + return false; + } + this->dataPtr->spawnPreviewModel = + this->dataPtr->renderUtil.SceneManager().CreateModel( + modelId, model, + this->dataPtr->renderUtil.SceneManager().WorldId()); + + this->dataPtr->modelIds.push_back(modelId); + for (auto j = 0u; j < model.LinkCount(); j++) + { + sdf::Link link = *(model.LinkByIndex(j)); + link.SetName(ignition::common::Uuid().String()); + Entity linkId = this->UniqueId(); + if (!linkId) + { + this->TerminatePreviewModel(); + return false; + } + this->dataPtr->renderUtil.SceneManager().CreateLink( + linkId, link, modelId); + this->dataPtr->modelIds.push_back(linkId); + for (auto k = 0u; k < link.VisualCount(); k++) + { + sdf::Visual visual = *(link.VisualByIndex(k)); + visual.SetName(ignition::common::Uuid().String()); + Entity visualId = this->UniqueId(); + if (!visualId) + { + this->TerminatePreviewModel(); + return false; + } + this->dataPtr->renderUtil.SceneManager().CreateVisual( + visualId, visual, linkId); + this->dataPtr->modelIds.push_back(visualId); + } + } + } + return true; +} + +///////////////////////////////////////////////// +void IgnRenderer::TerminatePreviewModel() { for (auto _id : this->dataPtr->modelIds) this->dataPtr->renderUtil.SceneManager().RemoveEntity(_id); @@ -844,7 +850,7 @@ void IgnRenderer::HandleModelPlacement() !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty) { // Delete the generated visuals - this->TerminateSpawnPreviewModel(); + this->TerminatePreviewModel(); sdf::Root root; root.LoadSdfString(this->dataPtr->modelSdfString); @@ -1660,17 +1666,10 @@ math::Vector3d IgnRenderer::ScreenToPlane( auto result = this->dataPtr->rayQuery->ClosestPoint(); ignition::math::Planed plane(ignition::math::Vector3d(0, 0, 1), 0); - if (result) - { - math::Vector3d origin = this->dataPtr->rayQuery->Origin(); - math::Vector3d direction = this->dataPtr->rayQuery->Direction(); - double distance = plane.Distance(origin, direction); - return origin + direction * distance; - } - - // Set point to be 10m away if no intersection found - return this->dataPtr->rayQuery->Origin() + - this->dataPtr->rayQuery->Direction() * 10; + math::Vector3d origin = this->dataPtr->rayQuery->Origin(); + math::Vector3d direction = this->dataPtr->rayQuery->Direction(); + double distance = plane.Distance(origin, direction); + return origin + direction * distance; } ///////////////////////////////////////////////// @@ -2142,14 +2141,6 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignmsg << "View angle service on [" << this->dataPtr->viewAngleService << "]" << std::endl; - // shapes - this->dataPtr->shapesService = - "/gui/shapes"; - this->dataPtr->node.Advertise(this->dataPtr->shapesService, - &Scene3D::OnShapes, this); - ignmsg << "Shapes service on [" - << this->dataPtr->shapesService << "]" << std::endl; - ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->installEventFilter(this); } @@ -2241,18 +2232,6 @@ bool Scene3D::OnViewAngle(const msgs::Vector3d &_msg, return true; } -///////////////////////////////////////////////// -bool Scene3D::OnShapes(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetModel(msgs::Convert(_msg)); - - _res.set_data(true); - return true; -} - ///////////////////////////////////////////////// void Scene3D::OnDropped(const QString &_drop, int _mouseX, int _mouseY) { @@ -2367,6 +2346,17 @@ bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) renderWindow->SetScaleSnap(snapEvent->Scale()); } } + else if (_event->type() == + ignition::gazebo::gui::events::SpawnPreviewModel::kType) + { + auto spawnPreviewModelEvent = + reinterpret_cast(_event); + if (spawnPreviewModelEvent) + { + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->SetModel(spawnPreviewModelEvent->ModelSdfString()); + } + } // Standard event processing return QObject::eventFilter(_obj, _event); @@ -2558,7 +2548,7 @@ void RenderWindowItem::keyReleaseEvent(QKeyEvent *_e) _e->accept(); } this->DeselectAllEntities(true); - this->dataPtr->renderThread->ignRenderer.TerminateSpawnPreviewModel(); + // TODO(john): Delete model preview on Escape press } } diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 3087e3b467..255872f52e 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -137,13 +137,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: bool OnViewAngle(const msgs::Vector3d &_msg, msgs::Boolean &_res); - /// \brief Callback for a shapes request - /// \param[in] _msg Request message to set a new shape. - /// \param[in] _res Response data. - /// \return True if the request is received. - private: bool OnShapes(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; @@ -337,8 +330,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return The unique entity id private: Entity UniqueId(); + /// \brief Generate a preview of a model. + /// \param[in] _modelSdfString The sdf string of the model to be generated + /// \return True on success, false if failure + public: bool GeneratePreviewModel(const std::string &_modelSdfString); + /// \brief Delete the visuals generated from the shapes plugin. - public: void TerminateSpawnPreviewModel(); + public: void TerminatePreviewModel(); /// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a /// ray cast from the given 2D screen coordinates. diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index c5d54c3111..7aa4eeea13 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -64,135 +64,142 @@ void Shapes::LoadConfig(const tinyxml2::XMLElement *) if (this->title.empty()) this->title = "Shapes"; - // For transform requests - this->dataPtr->service = "/gui/shapes"; + // For shapes requests + ignition::gui::App()->findChild + ()->installEventFilter(this); } ///////////////////////////////////////////////// void Shapes::OnMode(const QString &_mode) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - ignerr << "Error setting shape" << std::endl; - }; - - ignition::msgs::StringMsg req; + std::string modelSdfString = _mode.toStdString(); + std::transform(modelSdfString.begin(), modelSdfString.end(), + modelSdfString.begin(), ::tolower); - std::string sdfString; - if (_mode == "box") + if (modelSdfString == "box") { - sdfString = std::string("" - "" - "" - "0 0 0.5 0 0 0" - "" - "" - "" - "0.167" - "0" - "0" - "0.167" - "0" - "0.167" - "" - "1.0" - "" - "" - "" - "" - "1 1 1" - "" - "" - "" - "" - "" - "" - "1 1 1" - "" - "" - "" - "" - "" - ""); + modelSdfString = std::string("" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + "" + "0.167" + "0" + "0" + "0.167" + "0" + "0.167" + "" + "1.0" + "" + "" + "" + "" + "1 1 1" + "" + "" + "" + "" + "" + "" + "1 1 1" + "" + "" + "" + "" + "" + ""); } - else if (_mode == "sphere") + else if (modelSdfString == "sphere") { - sdfString = std::string("" - "" - "" - "0 0 0.5 0 0 0" - "" - "" - "" - "0.1" - "0" - "0" - "0.1" - "0" - "0.1" - "" - "1.0" - "" - "" - "" - "" - "0.5" - "" - "" - "" - "" - "" - "" - "0.5" - "" - "" - "" - "" - "" - ""); + modelSdfString = std::string("" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + "" + "0.1" + "0" + "0" + "0.1" + "0" + "0.1" + "" + "1.0" + "" + "" + "" + "" + "0.5" + "" + "" + "" + "" + "" + "" + "0.5" + "" + "" + "" + "" + "" + ""); } - else if (_mode == "cylinder") + else if (modelSdfString == "cylinder") { - sdfString = std::string("" - "" - "" - "0 0 0.5 0 0 0" - "" - "" - "" - "0.146" - "0" - "0" - "0.146" - "0" - "0.125" - "" - "1.0" - "" - "" - "" - "" - "0.5" - "1.0" - "" - "" - "" - "" - "" - "" - "0.5" - "1.0" - "" - "" - "" - "" - "" - ""); + modelSdfString = std::string("" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + "" + "0.146" + "0" + "0" + "0.146" + "0" + "0.125" + "" + "1.0" + "" + "" + "" + "" + "0.5" + "1.0" + "" + "" + "" + "" + "" + "" + "0.5" + "1.0" + "" + "" + "" + "" + "" + ""); } - req.set_data(sdfString); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); + else + { + ignwarn << "Invalid model string " << modelSdfString << "\n"; + ignwarn << "The valid options are:\n"; + ignwarn << " - box\n"; + ignwarn << " - sphere\n"; + ignwarn << " - cylinder\n"; + return; + } + + auto event = new gui::events::SpawnPreviewModel(modelSdfString); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + event); } // Register this plugin From 88456dafdb05dbba2875810d90b1562c17433134 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Sun, 10 May 2020 15:56:16 -0700 Subject: [PATCH 14/18] Add suggested fixes Signed-off-by: John Shepherd --- include/ignition/gazebo/rendering/SceneManager.hh | 2 +- src/gui/plugins/scene3d/Scene3D.cc | 13 ++++++++----- src/gui/plugins/shapes/Shapes.qml | 2 -- src/rendering/SceneManager.cc | 2 +- 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 65e9e663f7..27d3f7fdac 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -66,7 +66,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Get the world's ID. /// \return World ID - public: Entity WorldId(); + public: Entity WorldId() const; /// \brief Create a model /// \param[in] _id Unique model id diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 31e2849a55..3a5f8bbf7a 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -225,6 +225,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief The sdf string of the model to be used with the shapes plugin public: std::string modelSdfString; + /// \brief The pose of the preview model + public: ignition::math::Pose3d previewModelPose = + ignition::math::Pose3d::Zero; + /// \brief The currently hovered mouse position in screen coordinates public: math::Vector2i mouseHoverPos = math::Vector2i::Zero; @@ -613,9 +617,12 @@ bool IgnRenderer::GeneratePreviewModel(const std::string &_modelSdfString) { sdf::Root root; root.LoadSdfString(_modelSdfString); + for (auto i = 0u; i < root.ModelCount(); i++) { sdf::Model model = *(root.ModelByIndex(i)); + if (i == 0) + this->dataPtr->previewModelPose = model.Pose(); model.SetName(ignition::common::Uuid().String()); Entity modelId = this->UniqueId(); if (!modelId) @@ -852,10 +859,7 @@ void IgnRenderer::HandleModelPlacement() // Delete the generated visuals this->TerminatePreviewModel(); - sdf::Root root; - root.LoadSdfString(this->dataPtr->modelSdfString); - sdf::Model model = *(root.ModelByIndex(0)); - math::Pose3d modelPose = model.Pose(); + math::Pose3d modelPose = this->dataPtr->previewModelPose; std::function cb = [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) { @@ -1663,7 +1667,6 @@ math::Vector3d IgnRenderer::ScreenToPlane( this->dataPtr->rayQuery->SetFromCamera( this->dataPtr->camera, math::Vector2d(nx, ny)); - auto result = this->dataPtr->rayQuery->ClosestPoint(); ignition::math::Planed plane(ignition::math::Vector3d(0, 0, 1), 0); math::Vector3d origin = this->dataPtr->rayQuery->Origin(); diff --git a/src/gui/plugins/shapes/Shapes.qml b/src/gui/plugins/shapes/Shapes.qml index 70819c9cf4..afee977ed8 100644 --- a/src/gui/plugins/shapes/Shapes.qml +++ b/src/gui/plugins/shapes/Shapes.qml @@ -53,7 +53,6 @@ ToolBar { } ToolButton{ id: sphere - ButtonGroup.group: group ToolTip.text: "Sphere" ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval @@ -71,7 +70,6 @@ ToolBar { } ToolButton { id: cylinder - ButtonGroup.group: group ToolTip.text: "Cylinder" ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 5b4cc5af0e..361ab8315a 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -90,7 +90,7 @@ void SceneManager::SetWorldId(Entity _id) } ///////////////////////////////////////////////// -Entity SceneManager::WorldId() +Entity SceneManager::WorldId() const { return this->dataPtr->worldId; } From ace7cf6488eee83643a6550dc20667f92aa220e2 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Tue, 12 May 2020 13:32:55 -0700 Subject: [PATCH 15/18] Fix hover issue Signed-off-by: John Shepherd --- src/gui/plugins/scene3d/GzScene3D.qml | 3 +++ src/gui/plugins/scene3d/Scene3D.cc | 22 ++++++++++++---------- src/gui/plugins/scene3d/Scene3D.hh | 14 +++++++++----- 3 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/gui/plugins/scene3d/GzScene3D.qml b/src/gui/plugins/scene3d/GzScene3D.qml index aeec5ce7fc..1f8289465f 100644 --- a/src/gui/plugins/scene3d/GzScene3D.qml +++ b/src/gui/plugins/scene3d/GzScene3D.qml @@ -40,6 +40,9 @@ Rectangle { onEntered: { GzScene3D.OnFocusWindow() } + onPositionChanged: { + GzScene3D.OnHovered(mouseArea.mouseX, mouseArea.mouseY); + } } RenderWindow { diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 3a5f8bbf7a..49b9485b7a 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -1635,10 +1635,10 @@ void IgnRenderer::OnViewAngleComplete() } ///////////////////////////////////////////////// -void IgnRenderer::NewHoverEvent(const math::Vector2i &_mousePos) +void IgnRenderer::NewHoverEvent(const math::Vector2i &_hoverPos) { std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->mouseHoverPos = _mousePos; + this->dataPtr->mouseHoverPos = _hoverPos; this->dataPtr->hoverDirty = true; } @@ -1839,7 +1839,6 @@ RenderWindowItem::RenderWindowItem(QQuickItem *_parent) done = true; this->setAcceptedMouseButtons(Qt::AllButtons); - this->setAcceptHoverEvents(true); this->setFlag(ItemHasContents); this->dataPtr->renderThread = new RenderThread(); } @@ -2235,6 +2234,13 @@ bool Scene3D::OnViewAngle(const msgs::Vector3d &_msg, return true; } +///////////////////////////////////////////////// +void Scene3D::OnHovered(int _mouseX, int _mouseY) +{ + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->OnHovered({_mouseX, _mouseY}); +} + ///////////////////////////////////////////////// void Scene3D::OnDropped(const QString &_drop, int _mouseX, int _mouseY) { @@ -2459,13 +2465,9 @@ void RenderWindowItem::SetWorldName(const std::string &_name) } ///////////////////////////////////////////////// -void RenderWindowItem::hoverMoveEvent(QHoverEvent *_e) +void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos) { - math::Vector2i pos = {_e->pos().x(), _e->pos().y()}; - math::Vector2i oldPos = {_e->oldPos().x(), _e->oldPos().y()}; - if (pos == oldPos) - return; - this->dataPtr->renderThread->ignRenderer.NewHoverEvent(pos); + this->dataPtr->renderThread->ignRenderer.NewHoverEvent(_hoverPos); } ///////////////////////////////////////////////// @@ -2551,7 +2553,7 @@ void RenderWindowItem::keyReleaseEvent(QKeyEvent *_e) _e->accept(); } this->DeselectAllEntities(true); - // TODO(john): Delete model preview on Escape press + this->dataPtr->renderThread->ignRenderer.TerminatePreviewModel(); } } diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 20e81986cb..d2912c42a1 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -96,6 +96,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public slots: void OnDropped(const QString &_drop, int _mouseX, int _mouseY); + /// \brief Callback when the mouse hovers to a new position. + /// \param[in] _mouseX x coordinate of the hovered mouse position. + /// \param[in] _mouseY y coordinate of the hovered mouse position. + public slots: void OnHovered(int _mouseX, int _mouseY); + /// \brief Callback when the mouse enters the render window to /// focus the window for mouse/key events public slots: void OnFocusWindow(); @@ -240,8 +245,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { const math::Vector2d &_drag = math::Vector2d::Zero); /// \brief New hover event triggered. - /// \param[in] _mousePos Mouse hover screen position - public: void NewHoverEvent(const math::Vector2i &_mousePos); + /// \param[in] _hoverPos Mouse hover screen position + public: void NewHoverEvent(const math::Vector2i &_hoverPos); /// \brief Handle key press event for snapping /// \param[in] _e The key event to process. @@ -548,12 +553,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return 3D coordinates of a point in the 3D scene. public: math::Vector3d ScreenToScene(const math::Vector2i &_screenPos); + public: void OnHovered(const ignition::math::Vector2i &_hoverPos); + /// \brief Slot called when thread is ready to be started public Q_SLOTS: void Ready(); - // Documentation inherited - protected: void hoverMoveEvent(QHoverEvent *_e) override; - // Documentation inherited protected: void mousePressEvent(QMouseEvent *_e) override; From ec2d828cd259b7d1053d4b63684b7c1a5f71fb06 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Tue, 12 May 2020 13:38:59 -0700 Subject: [PATCH 16/18] minor fixes Signed-off-by: John Shepherd --- src/gui/plugins/scene3d/Scene3D.cc | 8 ++++---- src/gui/plugins/shapes/Shapes.qml | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 49b9485b7a..ec896e9b2e 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -216,7 +216,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: bool viewAngle = false; /// \brief Flag for indicating whether we are in shapes mode or not - public: bool spawnModelMode = false; + public: bool spawnModel = false; /// \brief Flag for indicating whether the user is currently placing a /// model with the shapes plugin or not @@ -594,13 +594,13 @@ void IgnRenderer::Render() // Shapes { IGN_PROFILE("IgnRenderer::Render Shapes"); - if (this->dataPtr->spawnModelMode) + if (this->dataPtr->spawnModel) { rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); rendering::VisualPtr rootVis = scene->RootVisual(); this->dataPtr->placingModel = this->GeneratePreviewModel(this->dataPtr->modelSdfString); - this->dataPtr->spawnModelMode = false; + this->dataPtr->spawnModel = false; } } @@ -1533,7 +1533,7 @@ void IgnRenderer::SetTransformMode(const std::string &_mode) void IgnRenderer::SetModel(const std::string &_model) { std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->spawnModelMode = true; + this->dataPtr->spawnModel = true; this->dataPtr->modelSdfString = _model; } diff --git a/src/gui/plugins/shapes/Shapes.qml b/src/gui/plugins/shapes/Shapes.qml index afee977ed8..0db003a072 100644 --- a/src/gui/plugins/shapes/Shapes.qml +++ b/src/gui/plugins/shapes/Shapes.qml @@ -35,7 +35,6 @@ ToolBar { spacing: 2 ToolButton { id: box - ButtonGroup.group: group ToolTip.text: "Box" ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval From a0e82b06a679c7161e21296ec76926978c810b32 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Tue, 12 May 2020 15:30:45 -0700 Subject: [PATCH 17/18] Only take first model Signed-off-by: John Shepherd --- src/gui/plugins/scene3d/Scene3D.cc | 83 ++++++++++++++++-------------- 1 file changed, 43 insertions(+), 40 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index ec896e9b2e..c29b501c3d 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -618,51 +618,54 @@ bool IgnRenderer::GeneratePreviewModel(const std::string &_modelSdfString) sdf::Root root; root.LoadSdfString(_modelSdfString); - for (auto i = 0u; i < root.ModelCount(); i++) - { - sdf::Model model = *(root.ModelByIndex(i)); - if (i == 0) - this->dataPtr->previewModelPose = model.Pose(); - model.SetName(ignition::common::Uuid().String()); - Entity modelId = this->UniqueId(); - if (!modelId) + if (!root.ModelCount()) + { + this->TerminatePreviewModel(); + return false; + } + + // Only preview first model + sdf::Model model = *(root.ModelByIndex(0)); + this->dataPtr->previewModelPose = model.Pose(); + model.SetName(ignition::common::Uuid().String()); + Entity modelId = this->UniqueId(); + if (!modelId) + { + this->TerminatePreviewModel(); + return false; + } + this->dataPtr->spawnPreviewModel = + this->dataPtr->renderUtil.SceneManager().CreateModel( + modelId, model, + this->dataPtr->renderUtil.SceneManager().WorldId()); + + this->dataPtr->modelIds.push_back(modelId); + for (auto j = 0u; j < model.LinkCount(); j++) + { + sdf::Link link = *(model.LinkByIndex(j)); + link.SetName(ignition::common::Uuid().String()); + Entity linkId = this->UniqueId(); + if (!linkId) { this->TerminatePreviewModel(); return false; } - this->dataPtr->spawnPreviewModel = - this->dataPtr->renderUtil.SceneManager().CreateModel( - modelId, model, - this->dataPtr->renderUtil.SceneManager().WorldId()); - - this->dataPtr->modelIds.push_back(modelId); - for (auto j = 0u; j < model.LinkCount(); j++) + this->dataPtr->renderUtil.SceneManager().CreateLink( + linkId, link, modelId); + this->dataPtr->modelIds.push_back(linkId); + for (auto k = 0u; k < link.VisualCount(); k++) { - sdf::Link link = *(model.LinkByIndex(j)); - link.SetName(ignition::common::Uuid().String()); - Entity linkId = this->UniqueId(); - if (!linkId) - { - this->TerminatePreviewModel(); - return false; - } - this->dataPtr->renderUtil.SceneManager().CreateLink( - linkId, link, modelId); - this->dataPtr->modelIds.push_back(linkId); - for (auto k = 0u; k < link.VisualCount(); k++) - { - sdf::Visual visual = *(link.VisualByIndex(k)); - visual.SetName(ignition::common::Uuid().String()); - Entity visualId = this->UniqueId(); - if (!visualId) - { - this->TerminatePreviewModel(); - return false; - } - this->dataPtr->renderUtil.SceneManager().CreateVisual( - visualId, visual, linkId); - this->dataPtr->modelIds.push_back(visualId); - } + sdf::Visual visual = *(link.VisualByIndex(k)); + visual.SetName(ignition::common::Uuid().String()); + Entity visualId = this->UniqueId(); + if (!visualId) + { + this->TerminatePreviewModel(); + return false; + } + this->dataPtr->renderUtil.SceneManager().CreateVisual( + visualId, visual, linkId); + this->dataPtr->modelIds.push_back(visualId); } } return true; From 5d7f4ace35d7956efd3808f196e755433082d558 Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Tue, 12 May 2020 15:35:54 -0700 Subject: [PATCH 18/18] Add docs Signed-off-by: John Shepherd --- src/gui/plugins/scene3d/Scene3D.hh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index d2912c42a1..2ae15cbe32 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -553,6 +553,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return 3D coordinates of a point in the 3D scene. public: math::Vector3d ScreenToScene(const math::Vector2i &_screenPos); + /// \brief Called when the mouse hovers to a new position. + /// \param[in] _hoverPos 2D coordinates of the hovered mouse position on + /// the render window. public: void OnHovered(const ignition::math::Vector2i &_hoverPos); /// \brief Slot called when thread is ready to be started