3D plot GUI plugin

examples/worlds/plot_3d.sdf

@@ -0,0 +1,152 @@
+<?xml version="1.0" ?>
+<!--
+  Demo the Plot3D plugin.
+
+  Start this world and see how the pendulum's link's paths are plotted in the 3D scene as it moves.
+
+  The plugin is instantiated twice, once for each link of the pendulum.
+
+  Try out different parameters in each instance of the plugin to see how the plot changes.
+-->
+<sdf version="1.6">
+  <world name="shapes">
+
+    <gui fullscreen="0">
+
+      <!-- 3D scene -->
+      <plugin filename="GzScene3D" name="3D View">
+        <ignition-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <ambient_light>0.4 0.4 0.4</ambient_light>
+        <background_color>0.8 0.8 0.8</background_color>
+        <camera_pose>5 -1.5 3 0 0.37 2.8</camera_pose>
+      </plugin>
+
+      <!-- World control -->
+      <plugin filename="WorldControl" name="World control">
+        <ignition-gui>
+          <title>World control</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">72</property>
+          <property type="double" key="width">121</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="left" target="left"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <play_pause>true</play_pause>
+        <step>true</step>
+        <start_paused>true</start_paused>
+      </plugin>
+
+      <!-- World statistics -->
+      <plugin filename="WorldStats" name="World stats">
+        <ignition-gui>
+          <title>World stats</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">110</property>
+          <property type="double" key="width">290</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="right" target="right"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <sim_time>true</sim_time>
+        <real_time>true</real_time>
+        <real_time_factor>true</real_time_factor>
+        <iterations>true</iterations>
+      </plugin>
+
+      <!-- Entity tree -->
+      <plugin filename="EntityTree" name="Entity tree">
+      </plugin>
+
+      <!-- Plot 3D: upper link -->
+      <plugin filename="Plot3D" name="Plot 3D">
+        <ignition-gui>
+          <title>Upper link plot</title>
+        </ignition-gui>
+        <entity_name>Double_pendulum::upper_link</entity_name>
+        <color>0 0 1</color>
+        <offset>0 0 1</offset>
+        <maximum_points>20</maximum_points>
+        <minimum_distance>0.1</minimum_distance>
+      </plugin>
+
+      <!-- Plot 3D: lower link -->
+      <plugin filename="Plot3D" name="Plot 3D">
+        <ignition-gui>
+          <title>Lower link plot</title>
+        </ignition-gui>
+        <entity_name>Double_pendulum::lower_link</entity_name>
+        <color>0 1 0</color>
+        <offset>0 0 1</offset>
+        <maximum_points>40</maximum_points>
+        <minimum_distance>0.3</minimum_distance>
+      </plugin>
+    </gui>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <include>
+      <name>Double_pendulum</name>
+      <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base</uri>
+    </include>
+
+  </world>
+</sdf>

include/ignition/gazebo/Util.hh

@@ -18,6 +18,7 @@
 #define IGNITION_GAZEBO_UTIL_HH_
 
 #include <string>
+#include <unordered_set>
 #include <vector>
 
 #include <ignition/math/Pose3.hh>
@@ -51,6 +52,34 @@ namespace ignition
       const EntityComponentManager &_ecm, const std::string &_delim = "/",
       bool _includePrefix = true);
 
+    /// \brief Helper function to get an entity given its scoped name.
+    /// The scope may start at any level by default. For example, in this
+    /// hierarchy:
+    ///
+    /// world_name
+    ///  model_name
+    ///    link_name
+    ///
+    /// All these names will return the link entity:
+    ///
+    /// * world_name::model_name::link_name
+    /// * model_name::link_name
+    /// * link_name
+    ///
+    /// \param[in] _scopedName Entity's scoped name.
+    /// \param[in] _ecm Immutable reference to ECM.
+    /// \param[in] _relativeTo Entity that the scoped name is relative to. The
+    /// scoped name does not include the name of this entity. If not provided,
+    /// the scoped name could be relative to any entity.
+    /// \param[in] _delim Delimiter between names, defaults to "::", it can't
+    /// be empty.
+    /// \return All entities that match the scoped name and relative to
+    /// requirements, or an empty set otherwise.
+    std::unordered_set<Entity> IGNITION_GAZEBO_VISIBLE entitiesFromScopedName(
+      const std::string &_scopedName, const EntityComponentManager &_ecm,
+      Entity _relativeTo = kNullEntity,
+      const std::string &_delim = "::");
+
     /// \brief Generally, each entity will be of some specific high-level type,
     /// such as World, Sensor, Collision, etc, and one type only.
     /// The entity type is usually marked by having some component that

src/Util.cc

@@ -59,8 +59,16 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
 math::Pose3d worldPose(const Entity &_entity,
     const EntityComponentManager &_ecm)
 {
+  auto poseComp = _ecm.Component<components::Pose>(_entity);
+  if (nullptr == poseComp)
+  {
+    ignwarn << "Trying to get world pose from entity [" << _entity
+            << "], which doesn't have a pose component" << std::endl;
+    return math::Pose3d();
+  }
+
   // work out pose in world frame
-  math::Pose3d pose = _ecm.Component<components::Pose>(_entity)->Data();
+  math::Pose3d pose = poseComp->Data();
   auto p = _ecm.Component<components::ParentEntity>(_entity);
   while (p)
   {
@@ -125,6 +133,64 @@ std::string scopedName(const Entity &_entity,
   return result;
 }
 
+//////////////////////////////////////////////////
+std::unordered_set<Entity> entitiesFromScopedName(
+    const std::string &_scopedName, const EntityComponentManager &_ecm,
+    Entity _relativeTo, const std::string &_delim)
+{
+  if (_delim.empty())
+  {
+    ignwarn << "Can't process scoped name [" << _scopedName
+            << "] with empty delimiter." << std::endl;
+    return {};
+  }
+
+  // Split names
+  std::vector<std::string> names;
+  size_t pos1 = 0;
+  size_t pos2 = _scopedName.find(_delim);
+  while (pos2 != std::string::npos)
+  {
+    names.push_back(_scopedName.substr(pos1, pos2 - pos1));
+    pos1 = pos2 + _delim.length();
+    pos2 = _scopedName.find(_delim, pos1);
+  }
+  names.push_back(_scopedName.substr(pos1, _scopedName.size()-pos1));
+
+  // Holds current entities that match and is updated for each name
+  std::vector<Entity> resVector;
+
+  // If there's an entity we're relative to, treat it as the first level result
+  if (_relativeTo != kNullEntity)
+  {
+    resVector = {_relativeTo};
+  }
+
+  for (const auto &name : names)
+  {
+    std::vector<Entity> current;
+    if (resVector.empty())
+    {
+      current = _ecm.EntitiesByComponents(components::Name(name));
+    }
+    else
+    {
+      for (auto res : resVector)
+      {
+        auto matches = _ecm.EntitiesByComponents(components::Name(name),
+            components::ParentEntity(res));
+        std::copy(std::begin(matches), std::end(matches),
+            std::back_inserter(current));
+      }
+    }
+    if (current.empty())
+      return {};
+    resVector = current;
+  }
+
+  return std::unordered_set<Entity>(resVector.begin(), resVector.end());
+}
+
 //////////////////////////////////////////////////
 ComponentTypeId entityTypeId(const Entity &_entity,
     const EntityComponentManager &_ecm)

src/Util_TEST.cc

@@ -218,6 +218,88 @@ TEST(UtilTest, ScopedName)
   EXPECT_EQ(kNullEntity, gazebo::worldEntity(kNullEntity, ecm));
 }
 
+/////////////////////////////////////////////////
+TEST(UtilTest, EntitiesFromScopedName)
+{
+  EntityComponentManager ecm;
+
+  // banana 1
+  //  - orange 2
+  //    - plum 3
+  //      - grape 4
+  //        - pear 5
+  //          - plum 6
+  //  - grape 7
+  //    - pear 8
+  //      - plum 9
+  //        - pear 10
+  //  - grape 11
+  //    - pear 12
+  //      - orange 13
+  //        - orange 14
+  //    - pear 15
+
+  auto createEntity = [&ecm](const std::string &_name, Entity _parent) -> Entity
+  {
+    auto res = ecm.CreateEntity();
+    ecm.CreateComponent(res, components::Name(_name));
+    ecm.CreateComponent(res, components::ParentEntity(_parent));
+    return res;
+  };
+
+  auto banana1 = ecm.CreateEntity();
+  ecm.CreateComponent(banana1, components::Name("banana"));
+
+  auto orange2 = createEntity("orange", banana1);
+  auto plum3 = createEntity("plum", orange2);
+  auto grape4 = createEntity("grape", plum3);
+  auto pear5 = createEntity("pear", grape4);
+  auto plum6 = createEntity("plum", pear5);
+  auto grape7 = createEntity("grape", banana1);
+  auto pear8 = createEntity("pear", grape7);
+  auto plum9 = createEntity("plum", pear8);
+  auto pear10 = createEntity("pear", plum9);
+  auto grape11 = createEntity("grape", banana1);
+  auto pear12 = createEntity("pear", grape11);
+  auto orange13 = createEntity("orange", pear12);
+  auto orange14 = createEntity("orange", orange13);
+  auto pear15 = createEntity("pear", grape11);
+
+  auto checkEntities = [&ecm](const std::string &_scopedName,
+      Entity _relativeTo, const std::unordered_set<Entity> &_result,
+      const std::string &_delim)
+  {
+    auto res = gazebo::entitiesFromScopedName(_scopedName, ecm, _relativeTo,
+        _delim);
+    EXPECT_EQ(_result.size(), res.size()) << _scopedName;
+
+    for (auto it : _result)
+    {
+      EXPECT_NE(res.find(it), res.end()) << it << "  " << _scopedName;
+    }
+  };
+
+  checkEntities("watermelon", kNullEntity, {}, "::");
+  checkEntities("banana", kNullEntity, {banana1}, "::");
+  checkEntities("orange", kNullEntity, {orange2, orange13, orange14}, ":");
+  checkEntities("banana::orange", kNullEntity, {orange2}, "::");
+  checkEntities("banana::grape", kNullEntity, {grape7, grape11}, "::");
+  checkEntities("grape/pear", kNullEntity, {pear5, pear8, pear12, pear15}, "/");
+  checkEntities("grape...pear...plum", kNullEntity, {plum6, plum9}, "...");
+  checkEntities(
+      "banana::orange::plum::grape::pear::plum", kNullEntity, {plum6}, "::");
+  checkEntities(
+      "banana::orange::kiwi::grape::pear::plum", kNullEntity, {}, "::");
+  checkEntities("orange+orange", kNullEntity, {orange14}, "+");
+  checkEntities("orange", banana1, {orange2}, "::");
+  checkEntities("grape", banana1, {grape7, grape11}, "::");
+  checkEntities("orange", orange2, {}, "::");
+  checkEntities("orange", orange13, {orange14}, "::");
+  checkEntities("grape::pear::plum", plum3, {plum6}, "::");
+  checkEntities("pear", grape11, {pear12, pear15}, "==");
+  checkEntities("plum=pear", pear8, {pear10}, "=");
+}
+
 /////////////////////////////////////////////////
 TEST(UtilTest, EntityTypeId)
 {

src/gui/plugins/CMakeLists.txt

@@ -111,6 +111,7 @@ add_subdirectory(entity_tree)
 add_subdirectory(grid_config)
 add_subdirectory(joint_position_controller)
 add_subdirectory(playback_scrubber)
+add_subdirectory(plot_3d)
 add_subdirectory(resource_spawner)
 add_subdirectory(scene3d)
 add_subdirectory(shapes)

src/gui/plugins/plot_3d/CMakeLists.txt

@@ -0,0 +1,8 @@
+gz_add_gui_plugin(Plot3D
+  SOURCES
+    Plot3D.cc
+  QT_HEADERS
+    Plot3D.hh
+  TEST_SOURCES
+    Plot3D_TEST.cc
+)