diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index aa66219bb3..f3c9a31248 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -111,6 +111,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { std::string(const sdf::Sensor &, const std::string &)> _createSensorCb = {}); + /// \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. public: class SceneManager &SceneManager(); diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index cf195f86b3..2099ef7b0f 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -96,6 +96,19 @@ 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 VisualById(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.cc b/src/gui/plugins/modules/EntityContextMenu.cc index 5bd42b2510..e76cbd8cf1 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -47,6 +47,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; }; @@ -75,6 +78,9 @@ EntityContextMenu::EntityContextMenu() // For remove service requests this->dataPtr->removeService = "/world/default/remove"; + + // For view collisions service requests + this->dataPtr->viewCollisionsService = "/gui/view/collisions"; } ///////////////////////////////////////////////// @@ -146,6 +152,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 == "view_collisions") + { + 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..bb2f4e02be 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -37,6 +37,40 @@ 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 >" + 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("view_collisions", context.entity) + } + } } function open(_entity, _type, _x, _y) { @@ -47,6 +81,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 +97,11 @@ Item { removeMenu.enabled = true } + if (context.type == "model" || context.type == "link") + { + viewCollisionsMenu.enabled = true; + } + menu.open() } @@ -71,5 +111,3 @@ Item { property string type } } - - diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index b6e3dc9af1..af6c3ef967 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -231,6 +231,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; @@ -429,6 +432,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief mutex to protect the render condition variable /// Used when recording in lockstep mode. public: std::mutex renderMutex; + + /// \brief View collisions service + public: std::string viewCollisionsService; }; } } @@ -803,6 +809,31 @@ 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) + { + Entity targetEntity = + this->dataPtr->renderUtil.SceneManager().EntityFromNode(targetNode); + this->dataPtr->renderUtil.ViewCollisions(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewCollisionsTarget + << "] to view collisions" << std::endl; + } + + this->dataPtr->viewCollisionsTarget.clear(); + } + } + if (ignition::gui::App()) { gui::events::Render event; @@ -1914,6 +1945,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) { @@ -2618,6 +2656,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 *>()->QuickWindow()->installEventFilter(this); ignition::gui::App()->findChild< @@ -2770,6 +2815,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) { @@ -3025,6 +3082,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 cf966a4efe..97289bd412 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; @@ -245,6 +252,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The world pose to set the camera to. public: void SetMoveToPose(const math::Pose3d &_pose); + /// \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 /// \param[in] _gain Camera follow p gain. public: void SetFollowPGain(double _gain); @@ -580,6 +591,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The new camera pose in the world frame. public: void SetMoveToPose(const math::Pose3d &_pose); + /// \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 /// \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 01bcd7644f..1dde5ba785 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -43,6 +44,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" @@ -197,6 +199,30 @@ 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 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 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; + + /// \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; }; ////////////////////////////////////////////////// @@ -225,6 +251,42 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info, this->dataPtr->UpdateRenderingEntities(_ecm); this->dataPtr->RemoveRenderingEntities(_ecm, _info); this->dataPtr->markerManager.SetSimTime(_info.simTime); + this->dataPtr->FindCollisionLinks(_ecm); +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::FindCollisionLinks(const EntityComponentManager &_ecm) +{ + if (this->newCollisions.empty()) + return; + + for (const auto &entity : this->newCollisions) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId})) + { + links = _ecm.EntitiesByComponents(components::ParentEntity(entity), + components::Link()); + } + else if (_ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links.push_back(entity); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing collision must be a model or link" + << std::endl; + continue; + } + + this->newCollisionLinks.insert(this->newCollisionLinks.end(), + links.begin(), + links.end()); + } + this->newCollisions.clear(); } ////////////////////////////////////////////////// @@ -263,6 +325,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(); @@ -274,6 +337,7 @@ void RenderUtil::Update() this->dataPtr->entityPoses.clear(); this->dataPtr->actorTransforms.clear(); this->dataPtr->entityTemp.clear(); + this->dataPtr->newCollisionLinks.clear(); this->dataPtr->markerManager.Update(); @@ -457,6 +521,43 @@ void RenderUtil::Update() visual->SetUserData("temperature", temp.second); } } + + // create new collision visuals + { + for (const auto &link : newCollisionLinks) + { + std::vector colEntities = + this->dataPtr->linkToCollisionEntities[link]; + + for (const auto &colEntity : colEntities) + { + if (!this->dataPtr->sceneManager.HasEntity(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(); + } + } + } + } + } + } } ////////////////////////////////////////////////// @@ -536,6 +637,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; }); @@ -604,6 +707,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->linkToCollisionEntities[_parent->Data()].push_back(_entity); + return true; + }); + if (this->enableSensors) { // Create cameras @@ -709,6 +829,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; }); @@ -768,6 +890,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->linkToCollisionEntities[_parent->Data()].push_back(_entity); + return true; + }); + if (this->enableSensors) { // Create cameras @@ -944,6 +1083,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( [&](const Entity &_entity, const components::Model *)->bool { this->removeEntities[_entity] = _info.iterations; + this->modelToLinkEntities.erase(_entity); return true; }); @@ -951,6 +1091,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( [&](const Entity &_entity, const components::Link *)->bool { this->removeEntities[_entity] = _info.iterations; + this->linkToCollisionEntities.erase(_entity); return true; }); @@ -1009,6 +1150,16 @@ 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; + this->viewingCollisions.erase(_entity); + this->entityCollisions.erase(_entity); + return true; + }); } ///////////////////////////////////////////////// @@ -1312,3 +1463,65 @@ void RenderUtilPrivate::LowlightNode(const rendering::NodePtr &_node) } } } + +///////////////////////////////////////////////// +void RenderUtil::ViewCollisions(const Entity &_entity) +{ + 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 (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; + // first loop looks for new collisions + for (const auto &colEntity : colEntities) + { + if (this->dataPtr->viewingCollisions.find(colEntity) == + this->dataPtr->viewingCollisions.end()) + { + this->dataPtr->newCollisions.push_back(_entity); + showColInit = showCol = true; + } + } + + // 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 + if (!showColInit) + { + showCol = !this->dataPtr->viewingCollisions[colEntity]; + showColInit = true; + } + + rendering::VisualPtr colVisual = + this->dataPtr->sceneManager.VisualById(colEntity); + if (colVisual == nullptr) + { + ignerr << "Could not find collision visual for entity [" << colEntity + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingCollisions[colEntity] = showCol; + colVisual->SetVisible(showCol); + } +} diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 7e0beb34b4..0be6da0be2 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -341,6 +342,36 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, return visualVis; } +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::VisualById(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::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); + + visual.SetRawPose(_collision.RawPose()); + visual.SetName(_collision.Name()); + + rendering::VisualPtr collisionVis = CreateVisual(_id, visual, _parentId); + return collisionVis; +} ///////////////////////////////////////////////// rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, math::Vector3d &_scale, math::Pose3d &_localPose)