From 93360d2509f49b84bfb42f714b5dd3ab699345d5 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Thu, 10 Dec 2020 18:39:59 -0800 Subject: [PATCH 01/11] init visualizing collisions Signed-off-by: Jenn Nguyen --- .../ignition/gazebo/rendering/RenderUtil.hh | 4 + src/gui/plugins/modules/EntityContextMenu.cc | 12 ++ src/gui/plugins/modules/EntityContextMenu.qml | 18 ++- src/gui/plugins/scene3d/Scene3D.cc | 61 +++++++++ src/gui/plugins/scene3d/Scene3D.hh | 13 ++ src/rendering/RenderUtil.cc | 126 ++++++++++++++++++ 6 files changed, 232 insertions(+), 2 deletions(-) diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 72a0055e79..7f96efb9ee 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -111,6 +112,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { std::string(const sdf::Sensor &, const std::string &)> _createSensorCb = {}); + // TODO(jenn) doc + public: void SetViewCollisions(const bool &_viewCollision, const Entity &_entity); + /// \brief Get the scene manager /// Returns reference to the scene manager. public: class SceneManager &SceneManager(); diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index eaa7804e86..457b7fe15a 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -44,6 +44,9 @@ namespace ignition::gazebo /// \brief Remove service name public: std::string removeService; + /// \brief View collisions service name + public: std::string viewCollisionsService; + /// \brief Name of world. public: std::string worldName; }; @@ -72,6 +75,9 @@ EntityContextMenu::EntityContextMenu() // For remove service requests this->dataPtr->removeService = "/world/default/remove"; + + // For view collisions service requests + this->dataPtr->viewCollisionsService = "/gui/view/collisions"; } ///////////////////////////////////////////////// @@ -143,6 +149,12 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->followService, req, cb); } + else if (request == "viewCollisions") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewCollisionsService, req, cb); + } else { ignwarn << "Unknown request [" << request << "]" << std::endl; diff --git a/src/gui/plugins/modules/EntityContextMenu.qml b/src/gui/plugins/modules/EntityContextMenu.qml index 9e927e2356..e6075da6d6 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -37,6 +37,15 @@ Item { text: "Remove" onTriggered: context.OnRemove(context.entity, context.type) } + Menu { + id: viewSubmenu + title: "View" + MenuItem { + id: viewCollisionsMenu + text: "Collisions" + onTriggered: context.OnRequest("viewCollisions", context.entity) + } + } } function open(_entity, _type, _x, _y) { @@ -47,6 +56,7 @@ Item { moveToMenu.enabled = false followMenu.enabled = false removeMenu.enabled = false + viewCollisionsMenu.enabled = false; // enable / disable menu items if (context.type == "model" || context.type == "link" || @@ -62,6 +72,12 @@ Item { removeMenu.enabled = true } + if (context.type == "model" || context.type == "link" || + context.type == "collision" ) + { + viewCollisionsMenu.enabled = true; + } + menu.open() } @@ -71,5 +87,3 @@ Item { property string type } } - - diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index e07dddb363..95c93d55db 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -200,6 +200,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Helper object to move user camera public: MoveToHelper moveToHelper; + /// \brief Target to view collisions + public: std::string viewCollisionsTarget; + /// \brief Helper object to select entities. Only the latest selection /// request is kept. public: SelectionHelper selectionHelper; @@ -375,6 +378,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Camera pose publisher public: transport::Node::Publisher cameraPosePub; + + /// \brief View collisions service + public: std::string viewCollisionsService; }; } } @@ -669,6 +675,29 @@ void IgnRenderer::Render() } } + // View collisions + { + IGN_PROFILE("IgnRenderer::Render ViewCollisions"); + if (!this->dataPtr->viewCollisionsTarget.empty()) + { + rendering::NodePtr targetNode = scene->NodeByName( + this->dataPtr->viewCollisionsTarget); + + if (!targetNode) + { + // TODO(jenn) fix error + std::cout << "No target" << std::endl; + } + else + { + Entity targetEntity = this->dataPtr->renderUtil.SceneManager().EntityFromNode(targetNode); + this->dataPtr->renderUtil.SetViewCollisions(true, targetEntity); + } + + this->dataPtr->viewCollisionsTarget.clear(); + } + } + if (ignition::gui::App()) { gui::events::Render event; @@ -1693,6 +1722,13 @@ void IgnRenderer::SetMoveToPose(const math::Pose3d &_pose) this->dataPtr->moveToPoseValue = _pose; } +///////////////////////////////////////////////// +void IgnRenderer::SetViewCollisionsTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewCollisionsTarget = _target; +} + ///////////////////////////////////////////////// void IgnRenderer::SetFollowPGain(double _gain) { @@ -2343,6 +2379,13 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignmsg << "Camera pose topic advertised on [" << this->dataPtr->cameraPoseTopic << "]" << std::endl; + // view collisions service + this->dataPtr->viewCollisionsService = "/gui/view/collisions"; + this->dataPtr->node.Advertise(this->dataPtr->viewCollisionsService, + &Scene3D::OnViewCollisions, this); + ignmsg << "View collisions service on [" + << this->dataPtr->viewCollisionsService << "]" << std::endl; + ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->installEventFilter(this); } @@ -2480,6 +2523,18 @@ bool Scene3D::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res) return true; } +///////////////////////////////////////////////// +bool Scene3D::OnViewCollisions(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewCollisionsTarget(_msg.data()); + + _res.set_data(true); + return true; +} + ///////////////////////////////////////////////// void Scene3D::OnHovered(int _mouseX, int _mouseY) { @@ -2697,6 +2752,12 @@ void RenderWindowItem::SetMoveToPose(const math::Pose3d &_pose) this->dataPtr->renderThread->ignRenderer.SetMoveToPose(_pose); } +///////////////////////////////////////////////// +void RenderWindowItem::SetViewCollisionsTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewCollisionsTarget(_target); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetFollowPGain(double _gain) { diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 5adf01854e..a182b84242 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -154,6 +154,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: bool OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res); + /// \brief Callback for view collisions request + /// \param[in] _msg Request message to set the target to view collisions + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewCollisions(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; @@ -228,6 +235,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The world pose to set the camera to. public: void SetMoveToPose(const math::Pose3d &_pose); + /// TODO(jenn) doc + public: void SetViewCollisionsTarget(const std::string &_target); + /// \brief Set the p gain for the camera follow movement /// \param[in] _gain Camera follow p gain. public: void SetFollowPGain(double _gain); @@ -537,6 +547,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The new camera pose in the world frame. public: void SetMoveToPose(const math::Pose3d &_pose); + /// TODO(jenn) doc + public: void Target(const std::string &_target); + /// \brief Set the p gain for the camera follow movement /// \param[in] _gain Camera follow p gain. public: void SetFollowPGain(double _gain); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index baad8e79a3..c18a10f9de 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -43,6 +43,7 @@ #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CastShadows.hh" +#include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Geometry.hh" @@ -152,6 +153,24 @@ class ignition::gazebo::RenderUtilPrivate public: std::vector> newSensors; + // TODO(jenn) doc + // key entity (collision), value tuple SDF DOM and parent entity id + public: std::map entityCollisions; + + // TODO(jenn) doc + // key parent (link) entity, value collision entity + public: std::map linkToCollisionEntity; + + // TODO(jenn) doc + public: bool viewCollisions = false; + + // TODO(jenn) doc + // key entity, value boolean if currently viewing in scene + public: std::map viewingCollisions; + + // TODO(jenn) doc + public: void Collisions(const EntityComponentManager &_ecm); + /// \brief Map of ids of entites to be removed and sim iteration when the /// remove request is received public: std::map removeEntities; @@ -225,6 +244,49 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info, this->dataPtr->UpdateRenderingEntities(_ecm); this->dataPtr->RemoveRenderingEntities(_ecm, _info); this->dataPtr->markerManager.SetSimTime(_info.simTime); + + if (this->dataPtr->viewCollisions) + { + this->dataPtr->Collisions(_ecm); + } +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::Collisions(const EntityComponentManager &_ecm) +{ + for (auto const& [entity, viewing] : this->viewingCollisions) + { + // don't do anything if collision is already in scene + if (viewing) continue; + + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId})) + { + auto links = _ecm.ChildrenByComponents(entity, components::Model()); + std::cout << "links.size: " << links.size() << std::endl; + } + else if (_ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + Entity colEntity = this->linkToCollisionEntity[entity]; + + if (!this->sceneManager.HasEntity(colEntity)) + { + sdf::Collision collision = this->entityCollisions[colEntity]; + + sdf::Material material; + material.SetAmbient(math::Color(1, 0.5088, 0.0468, 1)); + material.SetDiffuse(math::Color(1, 0.5088, 0.0468, 1)); + material.SetSpecular(math::Color(0.5, 0.5, 0.5, 1)); + + sdf::Visual visual; + visual.SetMaterial(material); + visual.SetGeom(*collision.Geom()); + + this->sceneManager.CreateVisual(colEntity, visual, entity); + } + } + } } ////////////////////////////////////////////////// @@ -604,6 +666,23 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); + // collisions + _ecm.Each( + [&](const Entity &_entity, + const components::Collision *, + const components::Name *, + const components::Pose *, + const components::Geometry *, + const components::CollisionElement *_collElement, + const components::ParentEntity *_parent) -> bool + { + this->entityCollisions[_entity] = _collElement->Data(); + this->linkToCollisionEntity[_parent->Data()] = _entity; + return true; + }); + if (this->enableSensors) { // Create cameras @@ -768,6 +847,23 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); + // collisions + _ecm.EachNew( + [&](const Entity &_entity, + const components::Collision *, + const components::Name *, + const components::Pose *, + const components::Geometry *, + const components::CollisionElement *_collElement, + const components::ParentEntity *_parent) -> bool + { + this->entityCollisions[_entity] = _collElement->Data(); + this->linkToCollisionEntity[_parent->Data()] = _entity; + return true; + }); + if (this->enableSensors) { // Create cameras @@ -1009,6 +1105,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( this->removeEntities[_entity] = _info.iterations; return true; }); + + // collisions + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Collision *)->bool + { + this->removeEntities[_entity] = _info.iterations; + return true; + }); } ///////////////////////////////////////////////// @@ -1305,3 +1409,25 @@ void RenderUtilPrivate::LowlightNode(const rendering::NodePtr &_node) } } } + +///////////////////////////////////////////////// +void RenderUtil::SetViewCollisions(const bool &_viewCollision, const Entity &_entity) +{ + if (_viewCollision) + { + if (this->dataPtr->viewingCollisions.find(_entity) != + this->dataPtr->viewingCollisions.end()) + { + // TODO(jenn) fix to a proper warning/error + std::cout << "Already viewing collision for enitity[" << _entity << "]" << std::endl; + return; + } + + this->dataPtr->viewingCollisions[_entity] = false; + } + else + { + // TODO(jenn) remove from viewingCollisions + } + this->dataPtr->viewCollisions = _viewCollision; +} From d047bad07fe78b02366c2f8f8a7fb9a765353161 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Fri, 11 Dec 2020 14:55:24 -0800 Subject: [PATCH 02/11] missing method definition Signed-off-by: Jenn Nguyen --- src/gui/plugins/scene3d/Scene3D.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index a182b84242..14afa54d14 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -548,7 +548,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void SetMoveToPose(const math::Pose3d &_pose); /// TODO(jenn) doc - public: void Target(const std::string &_target); + public: void SetViewCollisionsTarget(const std::string &_target); /// \brief Set the p gain for the camera follow movement /// \param[in] _gain Camera follow p gain. From e24ce539ceba0f099359d1df7da7dae1685d2db3 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Tue, 5 Jan 2021 12:51:23 -0500 Subject: [PATCH 03/11] enable/disable visualizing collisions Signed-off-by: Jenn Nguyen --- .../ignition/gazebo/rendering/RenderUtil.hh | 5 +- .../ignition/gazebo/rendering/SceneManager.hh | 5 + src/gui/plugins/modules/EntityContextMenu.qml | 26 ++- src/gui/plugins/scene3d/Scene3D.cc | 25 ++- src/gui/plugins/scene3d/Scene3D.hh | 6 +- src/rendering/RenderUtil.cc | 162 ++++++++++++------ src/rendering/SceneManager.cc | 11 ++ 7 files changed, 168 insertions(+), 72 deletions(-) diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 7f96efb9ee..60afebca95 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -112,8 +112,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { std::string(const sdf::Sensor &, const std::string &)> _createSensorCb = {}); - // TODO(jenn) doc - public: void SetViewCollisions(const bool &_viewCollision, const Entity &_entity); + /// \brief View collisions of specified entity which are shown in orange + /// \param[in] _entity Entity to view collisions + public: void ViewCollisions(const Entity &_entity); /// \brief Get the scene manager /// Returns reference to the scene manager. diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index cf195f86b3..c9f132f96b 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -96,6 +96,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateVisual(Entity _id, const sdf::Visual &_visual, Entity _parentId = 0); + /// \brief Retrieve visual + /// \param[in] _id Unique visual (entity) id + /// \return Pointer to requested visual + public: rendering::VisualPtr GetVisual(Entity _id); + /// \brief Create an actor /// \param[in] _id Unique actor id /// \param[in] _visual Actor sdf dom diff --git a/src/gui/plugins/modules/EntityContextMenu.qml b/src/gui/plugins/modules/EntityContextMenu.qml index e6075da6d6..c28d716f9c 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -37,13 +37,27 @@ Item { text: "Remove" onTriggered: context.OnRemove(context.entity, context.type) } - Menu { + MenuItem { id: viewSubmenu - title: "View" - MenuItem { - id: viewCollisionsMenu - text: "Collisions" - onTriggered: context.OnRequest("viewCollisions", context.entity) + text: "View >" + MouseArea { + id: viewSubMouseArea + anchors.fill: parent + hoverEnabled: true + onEntered: secondMenu.open() + } + } + } + Menu { + id: secondMenu + x: menu.x + menu.width + y: menu.y + viewSubmenu.y + MenuItem { + id: viewCollisionsMenu + text: "Collisions" + onTriggered: { + menu.close() + context.OnRequest("viewCollisions", context.entity) } } } diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 95c93d55db..3c73e68023 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -680,18 +680,31 @@ void IgnRenderer::Render() IGN_PROFILE("IgnRenderer::Render ViewCollisions"); if (!this->dataPtr->viewCollisionsTarget.empty()) { - rendering::NodePtr targetNode = scene->NodeByName( - this->dataPtr->viewCollisionsTarget); + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewCollisionsTarget); if (!targetNode) { - // TODO(jenn) fix error - std::cout << "No target" << std::endl; + // workaround for selecting collision + size_t found = this->dataPtr->viewCollisionsTarget.find_last_of("::"); + if (found != std::string::npos) + { + targetNode = scene->NodeByName( + this->dataPtr->viewCollisionsTarget.substr(0, found-1)); + } + } + + if (targetNode) + { + Entity targetEntity = + this->dataPtr->renderUtil.SceneManager().EntityFromNode(targetNode); + this->dataPtr->renderUtil.ViewCollisions(targetEntity); } else { - Entity targetEntity = this->dataPtr->renderUtil.SceneManager().EntityFromNode(targetNode); - this->dataPtr->renderUtil.SetViewCollisions(true, targetEntity); + ignerr << "Unable to find node name [" + << this->dataPtr->viewCollisionsTarget + << "] to view collisions" << std::endl; } this->dataPtr->viewCollisionsTarget.clear(); diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 14afa54d14..a27f1cd2ac 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -235,7 +235,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The world pose to set the camera to. public: void SetMoveToPose(const math::Pose3d &_pose); - /// TODO(jenn) doc + /// \brief View collisions of the specified target + /// \param[in] _target Target to view collisions public: void SetViewCollisionsTarget(const std::string &_target); /// \brief Set the p gain for the camera follow movement @@ -547,7 +548,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The new camera pose in the world frame. public: void SetMoveToPose(const math::Pose3d &_pose); - /// TODO(jenn) doc + /// \brief View collisions of the specified target + /// \param[in] _target Target to view collisions public: void SetViewCollisionsTarget(const std::string &_target); /// \brief Set the p gain for the camera follow movement diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index c18a10f9de..9254c2a040 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -153,24 +153,6 @@ class ignition::gazebo::RenderUtilPrivate public: std::vector> newSensors; - // TODO(jenn) doc - // key entity (collision), value tuple SDF DOM and parent entity id - public: std::map entityCollisions; - - // TODO(jenn) doc - // key parent (link) entity, value collision entity - public: std::map linkToCollisionEntity; - - // TODO(jenn) doc - public: bool viewCollisions = false; - - // TODO(jenn) doc - // key entity, value boolean if currently viewing in scene - public: std::map viewingCollisions; - - // TODO(jenn) doc - public: void Collisions(const EntityComponentManager &_ecm); - /// \brief Map of ids of entites to be removed and sim iteration when the /// remove request is received public: std::map removeEntities; @@ -216,6 +198,26 @@ class ignition::gazebo::RenderUtilPrivate /// \param[in] _node Node to be restored. /// TODO(anyone) On future versions, use a bounding box instead public: void LowlightNode(const rendering::NodePtr &_node); + + /// \brief New collisions to be created + public: std::vector newCollisions; + + /// \brief Render collision entities (displayed in orange) + /// \param[in] _ecm The entity-component manager + public: void CreateCollisions(const EntityComponentManager &_ecm); + + /// \brief A map of collision entity ids and their SDF DOM + public: std::map entityCollisions; + + /// \brief A map of model entities and their corresponding children links + public: std::map> modelToLinkEntities; + + /// \brief A map of link entities and their corresponding children collisions + public: std::map> linkToCollisionEntities; + + /// \brief A map of created collision entities and if they are currently + /// visible + public: std::map viewingCollisions; }; ////////////////////////////////////////////////// @@ -244,49 +246,62 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info, this->dataPtr->UpdateRenderingEntities(_ecm); this->dataPtr->RemoveRenderingEntities(_ecm, _info); this->dataPtr->markerManager.SetSimTime(_info.simTime); - - if (this->dataPtr->viewCollisions) - { - this->dataPtr->Collisions(_ecm); - } + this->dataPtr->CreateCollisions(_ecm); } ////////////////////////////////////////////////// -void RenderUtilPrivate::Collisions(const EntityComponentManager &_ecm) +void RenderUtilPrivate::CreateCollisions(const EntityComponentManager &_ecm) { - for (auto const& [entity, viewing] : this->viewingCollisions) - { - // don't do anything if collision is already in scene - if (viewing) continue; + if (this->newCollisions.empty()) + return; + for (auto const& entity : this->newCollisions) + { + std::vector links; if (_ecm.EntityMatches(entity, std::set{components::Model::typeId})) { - auto links = _ecm.ChildrenByComponents(entity, components::Model()); - std::cout << "links.size: " << links.size() << std::endl; + links = _ecm.EntitiesByComponents(components::ParentEntity(entity), + components::Link()); } else if (_ecm.EntityMatches(entity, std::set{components::Link::typeId})) { - Entity colEntity = this->linkToCollisionEntity[entity]; + links.push_back(entity); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing collision must be a model or link" + << std::endl; + continue; + } - if (!this->sceneManager.HasEntity(colEntity)) - { - sdf::Collision collision = this->entityCollisions[colEntity]; + for (auto const& link : links) + { + std::vector colEntities = this->linkToCollisionEntities[link]; - sdf::Material material; - material.SetAmbient(math::Color(1, 0.5088, 0.0468, 1)); - material.SetDiffuse(math::Color(1, 0.5088, 0.0468, 1)); - material.SetSpecular(math::Color(0.5, 0.5, 0.5, 1)); + for (const auto& colEntity : colEntities) + { + if (!this->sceneManager.HasEntity(colEntity)) + { + sdf::Collision collision = this->entityCollisions[colEntity]; - sdf::Visual visual; - visual.SetMaterial(material); - visual.SetGeom(*collision.Geom()); + sdf::Material material; + material.SetAmbient(math::Color(1, 0.5088, 0.0468, 1)); + material.SetDiffuse(math::Color(1, 0.5088, 0.0468, 1)); + material.SetSpecular(math::Color(0.5, 0.5, 0.5, 1)); - this->sceneManager.CreateVisual(colEntity, visual, entity); + sdf::Visual visual; + visual.SetGeom(*collision.Geom()); + visual.SetMaterial(material); + this->sceneManager.CreateVisual(colEntity, visual, link); + this->viewingCollisions[colEntity] = true; + } } } } + this->newCollisions.clear(); } ////////////////////////////////////////////////// @@ -598,6 +613,8 @@ void RenderUtilPrivate::CreateRenderingEntities( link.SetRawPose(_pose->Data()); this->newLinks.push_back( std::make_tuple(_entity, link, _parent->Data())); + // used for collsions + this->modelToLinkEntities[_parent->Data()].push_back(_entity); return true; }); @@ -679,7 +696,7 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::ParentEntity *_parent) -> bool { this->entityCollisions[_entity] = _collElement->Data(); - this->linkToCollisionEntity[_parent->Data()] = _entity; + this->linkToCollisionEntities[_parent->Data()].push_back(_entity); return true; }); @@ -788,6 +805,8 @@ void RenderUtilPrivate::CreateRenderingEntities( link.SetRawPose(_pose->Data()); this->newLinks.push_back( std::make_tuple(_entity, link, _parent->Data())); + // used for collsions + this->modelToLinkEntities[_parent->Data()].push_back(_entity); return true; }); @@ -860,7 +879,7 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::ParentEntity *_parent) -> bool { this->entityCollisions[_entity] = _collElement->Data(); - this->linkToCollisionEntity[_parent->Data()] = _entity; + this->linkToCollisionEntities[_parent->Data()].push_back(_entity); return true; }); @@ -1111,6 +1130,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( [&](const Entity &_entity, const components::Collision *)->bool { this->removeEntities[_entity] = _info.iterations; + this->viewingCollisions.erase(_entity); return true; }); } @@ -1411,23 +1431,53 @@ void RenderUtilPrivate::LowlightNode(const rendering::NodePtr &_node) } ///////////////////////////////////////////////// -void RenderUtil::SetViewCollisions(const bool &_viewCollision, const Entity &_entity) +void RenderUtil::ViewCollisions(const Entity &_entity) { - if (_viewCollision) + std::vector colEntities; + if (this->dataPtr->linkToCollisionEntities.find(_entity) != + this->dataPtr->linkToCollisionEntities.end()) + { + colEntities = this->dataPtr->linkToCollisionEntities[_entity]; + } + else if (this->dataPtr->modelToLinkEntities.find(_entity) != + this->dataPtr->modelToLinkEntities.end()) + { + std::vector links = this->dataPtr->modelToLinkEntities[_entity]; + for (auto const& link : links) + colEntities.insert(colEntities.end(), + this->dataPtr->linkToCollisionEntities[link].begin(), + this->dataPtr->linkToCollisionEntities[link].end()); + } + + bool showCol, showColInit = false; + for (auto const &colEntity : colEntities) { - if (this->dataPtr->viewingCollisions.find(_entity) != + if (this->dataPtr->viewingCollisions.find(colEntity) == this->dataPtr->viewingCollisions.end()) { - // TODO(jenn) fix to a proper warning/error - std::cout << "Already viewing collision for enitity[" << _entity << "]" << std::endl; - return; + this->dataPtr->newCollisions.push_back(_entity); + showColInit = showCol = true; + continue; } - this->dataPtr->viewingCollisions[_entity] = false; - } - else - { - // TODO(jenn) remove from viewingCollisions + // when viewing multiple collisions (e.g. _entity is a model), + // boolean for view collisions is based on first colEntity in list + if (!showColInit) + { + showCol = !this->dataPtr->viewingCollisions[colEntity]; + showColInit = true; + } + + rendering::VisualPtr colVisual = + this->dataPtr->sceneManager.GetVisual(colEntity); + if (colVisual == nullptr) + { + ignerr << "Could not find collision visual for entity [" << colEntity + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingCollisions[colEntity] = showCol; + colVisual->SetVisible(showCol); } - this->dataPtr->viewCollisions = _viewCollision; } diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 7e0beb34b4..26ad206967 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -341,6 +341,17 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, return visualVis; } +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::GetVisual(Entity _id) +{ + if (this->dataPtr->visuals.find(_id) == this->dataPtr->visuals.end()) + { + ignerr << "Could not find visual for entity: " << _id << std::endl; + return nullptr; + } + return this->dataPtr->visuals[_id]; +} + ///////////////////////////////////////////////// rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, math::Vector3d &_scale, math::Pose3d &_localPose) From 1882b807714ccb0cd21b50eadc721b32eab03065 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Tue, 5 Jan 2021 16:29:58 -0500 Subject: [PATCH 04/11] moved creating collision visuals to rendering thread Signed-off-by: Jenn Nguyen --- src/gui/plugins/modules/EntityContextMenu.qml | 3 +- src/rendering/RenderUtil.cc | 68 +++++++++++-------- 2 files changed, 42 insertions(+), 29 deletions(-) diff --git a/src/gui/plugins/modules/EntityContextMenu.qml b/src/gui/plugins/modules/EntityContextMenu.qml index c28d716f9c..de3ffa88bf 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -86,8 +86,7 @@ Item { removeMenu.enabled = true } - if (context.type == "model" || context.type == "link" || - context.type == "collision" ) + if (context.type == "model" || context.type == "link") { viewCollisionsMenu.enabled = true; } diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 9254c2a040..7154694a78 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -202,9 +202,13 @@ class ignition::gazebo::RenderUtilPrivate /// \brief New collisions to be created public: std::vector newCollisions; - /// \brief Render collision entities (displayed in orange) + /// \brief Finds the links (collision parent) that are used to create child + /// collision visuals in RenderUtil::Update /// \param[in] _ecm The entity-component manager - public: void CreateCollisions(const EntityComponentManager &_ecm); + public: void FindCollisionLinks(const EntityComponentManager &_ecm); + + /// \brief A list of links used to create new collision visuals + public: std::vector newCollisionLinks; /// \brief A map of collision entity ids and their SDF DOM public: std::map entityCollisions; @@ -246,11 +250,11 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info, this->dataPtr->UpdateRenderingEntities(_ecm); this->dataPtr->RemoveRenderingEntities(_ecm, _info); this->dataPtr->markerManager.SetSimTime(_info.simTime); - this->dataPtr->CreateCollisions(_ecm); + this->dataPtr->FindCollisionLinks(_ecm); } ////////////////////////////////////////////////// -void RenderUtilPrivate::CreateCollisions(const EntityComponentManager &_ecm) +void RenderUtilPrivate::FindCollisionLinks(const EntityComponentManager &_ecm) { if (this->newCollisions.empty()) return; @@ -277,29 +281,9 @@ void RenderUtilPrivate::CreateCollisions(const EntityComponentManager &_ecm) continue; } - for (auto const& link : links) - { - std::vector colEntities = this->linkToCollisionEntities[link]; - - for (const auto& colEntity : colEntities) - { - if (!this->sceneManager.HasEntity(colEntity)) - { - sdf::Collision collision = this->entityCollisions[colEntity]; - - sdf::Material material; - material.SetAmbient(math::Color(1, 0.5088, 0.0468, 1)); - material.SetDiffuse(math::Color(1, 0.5088, 0.0468, 1)); - material.SetSpecular(math::Color(0.5, 0.5, 0.5, 1)); - - sdf::Visual visual; - visual.SetGeom(*collision.Geom()); - visual.SetMaterial(material); - this->sceneManager.CreateVisual(colEntity, visual, link); - this->viewingCollisions[colEntity] = true; - } - } - } + this->newCollisionLinks.insert(this->newCollisionLinks.end(), + links.begin(), + links.end()); } this->newCollisions.clear(); } @@ -340,6 +324,7 @@ void RenderUtil::Update() auto entityPoses = std::move(this->dataPtr->entityPoses); auto actorTransforms = std::move(this->dataPtr->actorTransforms); auto entityTemp = std::move(this->dataPtr->entityTemp); + auto newCollisionLinks = std::move(this->dataPtr->newCollisionLinks); this->dataPtr->newScenes.clear(); this->dataPtr->newModels.clear(); @@ -351,6 +336,7 @@ void RenderUtil::Update() this->dataPtr->entityPoses.clear(); this->dataPtr->actorTransforms.clear(); this->dataPtr->entityTemp.clear(); + this->dataPtr->newCollisionLinks.clear(); this->dataPtr->markerManager.Update(); @@ -534,6 +520,34 @@ void RenderUtil::Update() visual->SetUserData("temperature", temp.second); } } + + // create new collision visuals + { + for (auto const& link : newCollisionLinks) + { + std::vector colEntities = + this->dataPtr->linkToCollisionEntities[link]; + + for (const auto& colEntity : colEntities) + { + if (!this->dataPtr->sceneManager.HasEntity(colEntity)) + { + sdf::Collision collision = this->dataPtr->entityCollisions[colEntity]; + + sdf::Material material; + material.SetAmbient(math::Color(1, 0.5088, 0.0468, 1)); + material.SetDiffuse(math::Color(1, 0.5088, 0.0468, 1)); + material.SetSpecular(math::Color(0.5, 0.5, 0.5, 1)); + + sdf::Visual visual; + visual.SetGeom(*collision.Geom()); + visual.SetMaterial(material); + this->dataPtr->sceneManager.CreateVisual(colEntity, visual, link); + this->dataPtr->viewingCollisions[colEntity] = true; + } + } + } + } } ////////////////////////////////////////////////// From 6d06dafed433d613bc4d29cd52062cd86de0831a Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Tue, 5 Jan 2021 17:08:32 -0500 Subject: [PATCH 05/11] fixed encountered error after collision visual creation Signed-off-by: Jenn Nguyen --- src/rendering/RenderUtil.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 7154694a78..04aece5e25 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -523,6 +523,9 @@ void RenderUtil::Update() // create new collision visuals { + if (!newCollisionLinks.empty()) + DeselectAllEntities(); + for (auto const& link : newCollisionLinks) { std::vector colEntities = From 704af0dcc31636fa5f0a736752e55a1ec59bfd9a Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Tue, 12 Jan 2021 16:51:58 -0500 Subject: [PATCH 06/11] added CreateCollision function to SceneManager Signed-off-by: Jenn Nguyen --- .../ignition/gazebo/rendering/SceneManager.hh | 10 +++++++++- src/gui/plugins/modules/EntityContextMenu.cc | 2 +- src/gui/plugins/modules/EntityContextMenu.qml | 13 ++++++++++++- src/gui/plugins/scene3d/Scene3D.cc | 11 ----------- src/rendering/RenderUtil.cc | 15 +++------------ src/rendering/SceneManager.cc | 18 +++++++++++++++++- 6 files changed, 42 insertions(+), 27 deletions(-) diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index c9f132f96b..2099ef7b0f 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -96,10 +96,18 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateVisual(Entity _id, const sdf::Visual &_visual, Entity _parentId = 0); + /// \brief Create a collision visual + /// \param[in] _id Unique visual id + /// \param[in] _collision Collision sdf dom + /// \param[in] _parentId Parent id + /// \return Visual (collision) object created from the sdf dom + public: rendering::VisualPtr CreateCollision(Entity _id, + const sdf::Collision &_collision, Entity _parentId = 0); + /// \brief Retrieve visual /// \param[in] _id Unique visual (entity) id /// \return Pointer to requested visual - public: rendering::VisualPtr GetVisual(Entity _id); + public: rendering::VisualPtr VisualById(Entity _id); /// \brief Create an actor /// \param[in] _id Unique actor id diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index 0f507d2872..e76cbd8cf1 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -152,7 +152,7 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->followService, req, cb); } - else if (request == "viewCollisions") + else if (request == "view_collisions") { ignition::msgs::StringMsg req; req.set_data(_data.toStdString()); diff --git a/src/gui/plugins/modules/EntityContextMenu.qml b/src/gui/plugins/modules/EntityContextMenu.qml index de3ffa88bf..bb2f4e02be 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -37,6 +37,17 @@ Item { text: "Remove" onTriggered: context.OnRemove(context.entity, context.type) } + // // cascading submenu only works in Qt 5.10+ on focal + // Menu { + // id: viewSubmenu + // title: "View" + // MenuItem { + // id: viewCollisionsMenu + // text: "Collisions" + // onTriggered: context.OnRequest("view_collisions", context.entity) + // } + // } + // workaround for getting submenu's to work on bionic (< Qt 5.10) MenuItem { id: viewSubmenu text: "View >" @@ -57,7 +68,7 @@ Item { text: "Collisions" onTriggered: { menu.close() - context.OnRequest("viewCollisions", context.entity) + context.OnRequest("view_collisions", context.entity) } } } diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 5272932c6d..af6c3ef967 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -817,17 +817,6 @@ void IgnRenderer::Render() rendering::NodePtr targetNode = scene->NodeByName(this->dataPtr->viewCollisionsTarget); - if (!targetNode) - { - // workaround for selecting collision - size_t found = this->dataPtr->viewCollisionsTarget.find_last_of("::"); - if (found != std::string::npos) - { - targetNode = scene->NodeByName( - this->dataPtr->viewCollisionsTarget.substr(0, found-1)); - } - } - if (targetNode) { Entity targetEntity = diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 40e77bc7bb..6266f95f60 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -535,17 +535,8 @@ void RenderUtil::Update() { if (!this->dataPtr->sceneManager.HasEntity(colEntity)) { - sdf::Collision collision = this->dataPtr->entityCollisions[colEntity]; - - sdf::Material material; - material.SetAmbient(math::Color(1, 0.5088, 0.0468, 1)); - material.SetDiffuse(math::Color(1, 0.5088, 0.0468, 1)); - material.SetSpecular(math::Color(0.5, 0.5, 0.5, 1)); - - sdf::Visual visual; - visual.SetGeom(*collision.Geom()); - visual.SetMaterial(material); - this->dataPtr->sceneManager.CreateVisual(colEntity, visual, link); + this->dataPtr->sceneManager.CreateCollision(colEntity, + this->dataPtr->entityCollisions[colEntity], link); this->dataPtr->viewingCollisions[colEntity] = true; } } @@ -1493,7 +1484,7 @@ void RenderUtil::ViewCollisions(const Entity &_entity) } rendering::VisualPtr colVisual = - this->dataPtr->sceneManager.GetVisual(colEntity); + this->dataPtr->sceneManager.VisualById(colEntity); if (colVisual == nullptr) { ignerr << "Could not find collision visual for entity [" << colEntity diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 26ad206967..7905d4573e 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -342,7 +343,7 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, } ///////////////////////////////////////////////// -rendering::VisualPtr SceneManager::GetVisual(Entity _id) +rendering::VisualPtr SceneManager::VisualById(Entity _id) { if (this->dataPtr->visuals.find(_id) == this->dataPtr->visuals.end()) { @@ -352,6 +353,21 @@ rendering::VisualPtr SceneManager::GetVisual(Entity _id) return this->dataPtr->visuals[_id]; } +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::CreateCollision(Entity _id, + const sdf::Collision &_collision, Entity _parentId) +{ + sdf::Material material; + material.SetAmbient(math::Color(1, 0.5088, 0.0468, 0.7)); + material.SetDiffuse(math::Color(1, 0.5088, 0.0468, 0.7)); + + sdf::Visual visual; + visual.SetGeom(*_collision.Geom()); + visual.SetMaterial(material); + visual.SetCastShadows(false); + rendering::VisualPtr collisionVis = CreateVisual(_id, visual, _parentId); + return collisionVis; +} ///////////////////////////////////////////////// rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, math::Vector3d &_scale, math::Pose3d &_localPose) From 85223984950c065ecaf711efd05c13048fd0a547 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Tue, 12 Jan 2021 18:19:59 -0500 Subject: [PATCH 07/11] removed map items when entity is removed Signed-off-by: Jenn Nguyen --- src/rendering/RenderUtil.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 6266f95f60..686b1e7a1a 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -535,7 +535,7 @@ void RenderUtil::Update() { if (!this->dataPtr->sceneManager.HasEntity(colEntity)) { - this->dataPtr->sceneManager.CreateCollision(colEntity, + this->dataPtr->sceneManager.CreateCollision(colEntity, this->dataPtr->entityCollisions[colEntity], link); this->dataPtr->viewingCollisions[colEntity] = true; } @@ -1067,6 +1067,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( [&](const Entity &_entity, const components::Model *)->bool { this->removeEntities[_entity] = _info.iterations; + this->modelToLinkEntities.erase(_entity); return true; }); @@ -1074,6 +1075,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( [&](const Entity &_entity, const components::Link *)->bool { this->removeEntities[_entity] = _info.iterations; + this->linkToCollisionEntities.erase(_entity); return true; }); @@ -1139,6 +1141,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( { this->removeEntities[_entity] = _info.iterations; this->viewingCollisions.erase(_entity); + this->entityCollisions.erase(_entity); return true; }); } From 8cd6b9867bde81e82dec3d69f1b850be1ad412ad Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Wed, 13 Jan 2021 08:57:56 -0500 Subject: [PATCH 08/11] fixed view collisions toggle Signed-off-by: Jenn Nguyen --- src/rendering/RenderUtil.cc | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 686b1e7a1a..54368a0540 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -259,7 +259,7 @@ void RenderUtilPrivate::FindCollisionLinks(const EntityComponentManager &_ecm) if (this->newCollisions.empty()) return; - for (auto const& entity : this->newCollisions) + for (const auto &entity : this->newCollisions) { std::vector links; if (_ecm.EntityMatches(entity, @@ -526,12 +526,12 @@ void RenderUtil::Update() if (!newCollisionLinks.empty()) DeselectAllEntities(); - for (auto const& link : newCollisionLinks) + for (const auto &link : newCollisionLinks) { std::vector colEntities = this->dataPtr->linkToCollisionEntities[link]; - for (const auto& colEntity : colEntities) + for (const auto &colEntity : colEntities) { if (!this->dataPtr->sceneManager.HasEntity(colEntity)) { @@ -1461,14 +1461,17 @@ void RenderUtil::ViewCollisions(const Entity &_entity) this->dataPtr->modelToLinkEntities.end()) { std::vector links = this->dataPtr->modelToLinkEntities[_entity]; - for (auto const& link : links) + for (const auto &link : links) colEntities.insert(colEntities.end(), this->dataPtr->linkToCollisionEntities[link].begin(), this->dataPtr->linkToCollisionEntities[link].end()); } + // create and/or toggle collision visuals + bool showCol, showColInit = false; - for (auto const &colEntity : colEntities) + // first loop looks for new collisions + for (const auto &colEntity : colEntities) { if (this->dataPtr->viewingCollisions.find(colEntity) == this->dataPtr->viewingCollisions.end()) @@ -1477,6 +1480,14 @@ void RenderUtil::ViewCollisions(const Entity &_entity) showColInit = showCol = true; continue; } + } + + // second loop toggles already created collisions + for (const auto &colEntity : colEntities) + { + if (this->dataPtr->viewingCollisions.find(colEntity) == + this->dataPtr->viewingCollisions.end()) + continue; // when viewing multiple collisions (e.g. _entity is a model), // boolean for view collisions is based on first colEntity in list From b432081c6b9f1926285f448c5ff3a24a528cfcba Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Thu, 14 Jan 2021 14:27:41 -0500 Subject: [PATCH 09/11] removed unneeded continue Signed-off-by: Jenn Nguyen --- src/rendering/RenderUtil.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 54368a0540..9f39e86317 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1478,7 +1478,6 @@ void RenderUtil::ViewCollisions(const Entity &_entity) { this->dataPtr->newCollisions.push_back(_entity); showColInit = showCol = true; - continue; } } From df7f00942033bc20b6416f76df048df648daca2f Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Wed, 20 Jan 2021 13:59:44 -0500 Subject: [PATCH 10/11] added emissive for new colision vis geom Signed-off-by: Jenn Nguyen --- src/rendering/RenderUtil.cc | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 9f39e86317..2c45a9fda1 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -523,9 +523,6 @@ void RenderUtil::Update() // create new collision visuals { - if (!newCollisionLinks.empty()) - DeselectAllEntities(); - for (const auto &link : newCollisionLinks) { std::vector colEntities = @@ -535,9 +532,27 @@ void RenderUtil::Update() { if (!this->dataPtr->sceneManager.HasEntity(colEntity)) { - this->dataPtr->sceneManager.CreateCollision(colEntity, + auto vis = this->dataPtr->sceneManager.CreateCollision(colEntity, this->dataPtr->entityCollisions[colEntity], link); this->dataPtr->viewingCollisions[colEntity] = true; + + // add geometry material to originalEmissive map + for (auto g = 0u; g < vis->GeometryCount(); ++g) + { + auto geom = vis->GeometryByIndex(g); + + // Geometry material + auto geomMat = geom->Material(); + if (nullptr == geomMat) + continue; + + if (this->dataPtr->originalEmissive.find(geom->Name()) == + this->dataPtr->originalEmissive.end()) + { + this->dataPtr->originalEmissive[geom->Name()] = + geomMat->Emissive(); + } + } } } } From dbf7a0c067c434c779e429d08cac07ae701b504b Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Thu, 21 Jan 2021 09:28:10 -0500 Subject: [PATCH 11/11] fixed collision poses Signed-off-by: Jenn Nguyen --- include/ignition/gazebo/rendering/RenderUtil.hh | 1 - src/rendering/RenderUtil.cc | 1 + src/rendering/SceneManager.cc | 4 ++++ 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index d6ce2c37a3..f3c9a31248 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -21,7 +21,6 @@ #include #include -#include #include #include diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 2c45a9fda1..1dde5ba785 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 7905d4573e..0be6da0be2 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -365,6 +365,10 @@ rendering::VisualPtr SceneManager::CreateCollision(Entity _id, visual.SetGeom(*_collision.Geom()); visual.SetMaterial(material); visual.SetCastShadows(false); + + visual.SetRawPose(_collision.RawPose()); + visual.SetName(_collision.Name()); + rendering::VisualPtr collisionVis = CreateVisual(_id, visual, _parentId); return collisionVis; }