Visualize collisions (#531)

Signed-off-by: Jenn Nguyen <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>

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();

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

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;

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
   }
 }
-
-

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<std::mutex> 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<RenderWindowItem *>();
+
+  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)
 {

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<Scene3DPrivate> 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);