Visualize collisions

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