4 to 5 (#918)

Signed-off-by: Ashton Larkin <[email protected]>

examples/plugin/hello_world/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+
+find_package(ignition-cmake2 REQUIRED)
+
+project(Hello_world)
+
+ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register)
+set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+
+ign_find_package(ignition-gazebo5 REQUIRED)
+set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR})
+
+add_library(HelloWorld SHARED HelloWorld)
+set_property(TARGET HelloWorld PROPERTY CXX_STANDARD 17)
+target_link_libraries(HelloWorld
+  PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
+  PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER})

examples/plugin/hello_world/HelloWorld.cc

@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+// We'll use a string and the ignmsg command below for a brief example.
+// Remove these includes if your plugin doesn't need them.
+#include <string>
+#include <ignition/common/Console.hh>
+
+// This header is required to register plugins. It's good practice to place it
+// in the cc file, like it's done here.
+#include <ignition/plugin/Register.hh>
+
+// Don't forget to include the plugin's header.
+#include "HelloWorld.hh"
+
+// This is required to register the plugin. Make sure the interfaces match
+// what's in the header.
+IGNITION_ADD_PLUGIN(
+    hello_world::HelloWorld,
+    ignition::gazebo::System,
+    hello_world::HelloWorld::ISystemPostUpdate)
+
+using namespace hello_world;
+
+// Here we implement the PostUpdate function, which is called at every
+// iteration.
+void HelloWorld::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
+    const ignition::gazebo::EntityComponentManager &/*_ecm*/)
+{
+  // This is a simple example of how to get information from UpdateInfo.
+  std::string msg = "Hello, world! Simulation is ";
+  if (!_info.paused)
+    msg += "not ";
+  msg += "paused.";
+
+  // Messages printed with ignmsg only show when running with verbosity 3 or
+  // higher (i.e. ign gazebo -v 3)
+  ignmsg << msg << std::endl;
+}
+
+
+

examples/plugin/hello_world/HelloWorld.hh

@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef SYSTEM_PLUGIN_HELLOWORLD_HH_
+#define SYSTEM_PLUGIN_HELLOWORLD_HH_
+
+// The only required include in the header is this one.
+// All others will depend on what your plugin does.
+#include <ignition/gazebo/System.hh>
+
+// It's good practice to use a custom namespace for your project.
+namespace hello_world
+{
+  // This is the main plugin's class. It must inherit from System and at least
+  // one other interface.
+  // Here we use `ISystemPostUpdate`, which is used to get results after
+  // physics runs. The opposite of that, `ISystemPreUpdate`, would be used by
+  // plugins that want to send commands.
+  class HelloWorld:
+    public ignition::gazebo::System,
+    public ignition::gazebo::ISystemPostUpdate
+  {
+    // Plugins inheriting ISystemPostUpdate must implement the PostUpdate
+    // callback. This is called at every simulation iteration after the physics
+    // updates the world. The _info variable provides information such as time,
+    // while the _ecm provides an interface to all entities and components in
+    // simulation.
+    public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
+                const ignition::gazebo::EntityComponentManager &_ecm) override;
+  };
+}
+#endif

examples/plugin/hello_world/README.md

@@ -0,0 +1,42 @@
+# Hello world
+
+This example contains the bare minimum that's necessary to create a Gazebo
+system plugin.
+
+## Build
+
+From the root of the `ign-gazebo` repository, do the following to build the example:
+
+~~~
+cd ign-gazebo/examples/plugins/hello_world
+mkdir build
+cd build
+cmake ..
+make
+~~~
+
+This will generate the `HelloWorld` library under `build`.
+
+## Run
+
+The plugin must be attached to an entity to be loaded. This is demonstrated in
+the `hello_world_plugin.sdf` file that's going to be loaded.
+
+Before starting Gazebo, we must make sure it can find the plugin by doing:
+
+~~~
+cd ign-gazebo/examples/plugins/hello_world
+export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/build
+~~~
+
+Then load the example world:
+
+    ign gazebo -v 3 hello_world_plugin.sdf
+
+You should see green messages on the terminal like:
+
+```
+[Msg] Hello, world! Simulation is paused.
+```
+
+Toggle the play / pause buttons to see the message change.

examples/plugin/hello_world/hello_world_plugin.sdf

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+    <!--
+      System plugins can be loaded from <plugin> tags attached to entities like
+      the world, models, visuals, etc.
+    -->
+    <plugin filename="HelloWorld" name="hello_world::HelloWorld">
+    </plugin>
+  </world>
+</sdf>

include/ignition/gazebo/Model.hh

@@ -151,6 +151,12 @@ namespace ignition
       public: std::vector<gazebo::Entity> Links(
           const EntityComponentManager &_ecm) const;
 
+      /// \brief Get all models which are immediate children of this model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return All models in this model.
+      public: std::vector<gazebo::Entity> Models(
+          const EntityComponentManager &_ecm) const;
+
       /// \brief Get the number of joints which are immediate children of this
       /// model.
       /// \param[in] _ecm Entity-component manager.

include/ignition/gazebo/Types.hh

@@ -97,7 +97,7 @@ namespace ignition
     static const ComponentId kComponentIdInvalid = -1;
 
     /// \brief Id that indicates an invalid component type.
-    static const ComponentTypeId kComponentTypeIdInvalid = -1;
+    static const ComponentTypeId kComponentTypeIdInvalid = UINT64_MAX;
     }
   }
 }

include/ignition/gazebo/components/Sensor.hh

@@ -31,7 +31,7 @@ namespace gazebo
 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
 namespace components
 {
-  /// \brief A component that identifies an entity as being a link.
+  /// \brief A component that identifies an entity as being a sensor.
   using Sensor = Component<NoData, class SensorTag>;
   IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Sensor", Sensor)
 

src/Model.cc

@@ -163,6 +163,14 @@ std::vector<Entity> Model::Links(const EntityComponentManager &_ecm) const
       components::Link());
 }
 
+//////////////////////////////////////////////////
+std::vector<Entity> Model::Models(const EntityComponentManager &_ecm) const
+{
+  return _ecm.EntitiesByComponents(
+      components::ParentEntity(this->dataPtr->id),
+      components::Model());
+}
+
 //////////////////////////////////////////////////
 uint64_t Model::JointCount(const EntityComponentManager &_ecm) const
 {

src/Model_TEST.cc

@@ -17,7 +17,14 @@
 
 #include <gtest/gtest.h>
 
+#include <cstddef>
+
+#include "ignition/gazebo/EntityComponentManager.hh"
 #include "ignition/gazebo/Model.hh"
+#include "ignition/gazebo/components/Link.hh"
+#include "ignition/gazebo/components/Model.hh"
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/ParentEntity.hh"
 
 /////////////////////////////////////////////////
 TEST(ModelTest, Constructor)
@@ -74,3 +81,145 @@ TEST(ModelTest, MoveAssignmentOperator)
   modelMoved = std::move(model);
   EXPECT_EQ(id, modelMoved.Entity());
 }
+
+/////////////////////////////////////////////////
+TEST(ModelTest, Links)
+{
+  // modelA
+  //  - linkAA
+  //  - linkAB
+  //  - modelB
+  //    - linkBA
+  //
+  // modelC
+
+  ignition::gazebo::EntityComponentManager ecm;
+
+  // Model A
+  auto modelAEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelAEntity, ignition::gazebo::components::Model());
+  ecm.CreateComponent(modelAEntity,
+      ignition::gazebo::components::Name("modelA_name"));
+
+  // Link AA - Child of Model A
+  auto linkAAEntity = ecm.CreateEntity();
+  ecm.CreateComponent(linkAAEntity, ignition::gazebo::components::Link());
+  ecm.CreateComponent(linkAAEntity,
+      ignition::gazebo::components::Name("linkAA_name"));
+  ecm.CreateComponent(linkAAEntity,
+      ignition::gazebo::components::ParentEntity(modelAEntity));
+
+  // Link AB - Child of Model A
+  auto linkABEntity = ecm.CreateEntity();
+  ecm.CreateComponent(linkABEntity, ignition::gazebo::components::Link());
+  ecm.CreateComponent(linkABEntity,
+      ignition::gazebo::components::Name("linkAB_name"));
+  ecm.CreateComponent(linkABEntity,
+      ignition::gazebo::components::ParentEntity(modelAEntity));
+
+  // Model B - Child of Model A
+  auto modelBEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelBEntity, ignition::gazebo::components::Model());
+  ecm.CreateComponent(modelBEntity,
+      ignition::gazebo::components::Name("modelB_name"));
+  ecm.CreateComponent(modelBEntity,
+      ignition::gazebo::components::ParentEntity(modelAEntity));
+
+  // Link BA - Child of Model B
+  auto linkBAEntity = ecm.CreateEntity();
+  ecm.CreateComponent(linkBAEntity, ignition::gazebo::components::Link());
+  ecm.CreateComponent(linkBAEntity,
+      ignition::gazebo::components::Name("linkBA_name"));
+  ecm.CreateComponent(linkBAEntity,
+      ignition::gazebo::components::ParentEntity(modelBEntity));
+
+  // Model C
+  auto modelCEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelCEntity, ignition::gazebo::components::Model());
+  ecm.CreateComponent(modelCEntity,
+      ignition::gazebo::components::Name("modelC_name"));
+
+  std::size_t foundLinks = 0;
+
+  ignition::gazebo::Model modelA(modelAEntity);
+  auto links = modelA.Links(ecm);
+  EXPECT_EQ(2u, links.size());
+  for (const auto &link : links)
+  {
+    if (link == linkAAEntity || link == linkABEntity)
+      foundLinks++;
+  }
+  EXPECT_EQ(foundLinks, links.size());
+
+  ignition::gazebo::Model modelB(modelBEntity);
+  links = modelB.Links(ecm);
+  ASSERT_EQ(1u, links.size());
+  EXPECT_EQ(linkBAEntity, links[0]);
+
+  ignition::gazebo::Model modelC(modelCEntity);
+  EXPECT_EQ(0u, modelC.Links(ecm).size());
+}
+
+/////////////////////////////////////////////////
+TEST(ModelTest, Models)
+{
+  // modelA
+  //  - modelB
+  //  - modelC
+  //    - modelD
+
+  ignition::gazebo::EntityComponentManager ecm;
+
+  // Model A
+  auto modelAEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelAEntity, ignition::gazebo::components::Model());
+  ecm.CreateComponent(modelAEntity,
+      ignition::gazebo::components::Name("modelA_name"));
+
+  // Model B - Child of Model A
+  auto modelBEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelBEntity, ignition::gazebo::components::Model());
+  ecm.CreateComponent(modelBEntity,
+      ignition::gazebo::components::Name("modelB_name"));
+  ecm.CreateComponent(modelBEntity,
+      ignition::gazebo::components::ParentEntity(modelAEntity));
+
+  // Model C - Child of Model A
+  auto modelCEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelCEntity, ignition::gazebo::components::Model());
+  ecm.CreateComponent(modelCEntity,
+      ignition::gazebo::components::Name("modelC_name"));
+  ecm.CreateComponent(modelCEntity,
+      ignition::gazebo::components::ParentEntity(modelAEntity));
+
+  // Model D - Child of Model C
+  auto modelDEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelDEntity, ignition::gazebo::components::Model());
+  ecm.CreateComponent(modelDEntity,
+      ignition::gazebo::components::Name("modelD_name"));
+  ecm.CreateComponent(modelDEntity,
+      ignition::gazebo::components::ParentEntity(modelCEntity));
+
+  std::size_t foundModels = 0;
+
+  ignition::gazebo::Model modelA(modelAEntity);
+  auto models = modelA.Models(ecm);
+  EXPECT_EQ(2u, models.size());
+  for (const auto &model : models)
+  {
+    if (model == modelBEntity || model == modelCEntity)
+      foundModels++;
+  }
+  EXPECT_EQ(foundModels, models.size());
+
+  ignition::gazebo::Model modelB(modelBEntity);
+  EXPECT_EQ(0u, modelB.Models(ecm).size());
+
+  ignition::gazebo::Model modelC(modelCEntity);
+  models = modelC.Models(ecm);
+  ASSERT_EQ(1u, models.size());
+  EXPECT_EQ(modelDEntity, models[0]);
+
+  ignition::gazebo::Model modelD(modelDEntity);
+  EXPECT_EQ(0u, modelD.Models(ecm).size());
+}