[Citadel] Add Base and EntityManagement to tpeplugin

tpe/plugin/CMakeLists.txt

@@ -4,31 +4,52 @@ ign_add_component(tpe INTERFACE
   DEPENDS_ON_COMPONENTS sdf mesh
   GET_TARGET_NAME features)
 
-target_link_libraries(${features} INTERFACE ${PROJECT_LIBRARY_TARGET_NAME}-tpe)
+target_link_libraries(${features} INTERFACE ${PROJECT_LIBRARY_TARGET_NAME}-tpelib)
 target_include_directories(${features} SYSTEM INTERFACE)
 
 ign_get_libsources_and_unittests(sources test_sources)
 
 # TODO(MXG): Think about an ign_add_plugin(~) macro for ign-cmake
-ign_add_component(tpe-plugin
+set(engine_name tpe-plugin)
+ign_add_component(${engine_name}
   SOURCES ${sources}
   DEPENDS_ON_COMPONENTS tpe
   GET_TARGET_NAME tpe_plugin)
 
+set(tpelib_dir ${PROJECT_SOURCE_DIR}/tpe)
+target_include_directories(${tpe_plugin} PRIVATE ${tpelib_dir})
+
 target_link_libraries(${tpe_plugin}
   PUBLIC
     ${features}
     ${PROJECT_LIBRARY_TARGET_NAME}-sdf
     ${PROJECT_LIBRARY_TARGET_NAME}-mesh
     ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
-    ignition-math${IGN_MATH_VER}::eigen3)
-
-# We need to link this, even when the profiler isn't used to get headers.
-target_link_libraries(${tpe_plugin}
+    ignition-math${IGN_MATH_VER}::eigen3
   PRIVATE
-  ignition-common${IGN_COMMON_VER}::profiler
+    # We need to link this, even when the profiler isn't used to get headers.
+    ignition-common${IGN_COMMON_VER}::profiler
 )
 
+# Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir
+install(TARGETS ${tpe_plugin} DESTINATION ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR})
+
+# The library created by `ign_add_component` includes the ign-physics version
+# (i.e. libignition-physics1-name-plugin.so), but for portability,
+# we also install an unversioned symlink into the same versioned folder.
+set(versioned ${CMAKE_SHARED_LIBRARY_PREFIX}${tpe_plugin}${CMAKE_SHARED_LIBRARY_SUFFIX})
+set(unversioned ${CMAKE_SHARED_LIBRARY_PREFIX}${PROJECT_NAME_NO_VERSION_LOWER}-${engine_name}${CMAKE_SHARED_LIBRARY_SUFFIX})
+if (WIN32)
+  # disable MSVC inherit via dominance warning
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4250")
+  INSTALL(CODE "EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E copy
+      ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}\/${versioned} 
+      ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})")
+else()
+  EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned})
+  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR})
+endif()
+
 ign_build_tests(
   TYPE UNIT_tpe
   SOURCES ${test_sources}
@@ -42,11 +63,10 @@ ign_build_tests(
 
 foreach(test ${tests})
 
+  target_include_directories(${test} PRIVATE ${tpelib_dir})
   target_compile_definitions(${test} PRIVATE
     "tpe_plugin_LIB=\"$<TARGET_FILE:${tpe_plugin}>\""
     "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\""
     "IGNITION_PHYSICS_RESOURCE_DIR=\"${IGNITION_PHYSICS_RESOURCE_DIR}\"")
 
 endforeach()
-
-

tpe/plugin/src/Base.hh

@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2020 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 IGNITION_PHYSICS_TPE_PLUGIN_SRC_BASE_HH_
+#define IGNITION_PHYSICS_TPE_PLUGIN_SRC_BASE_HH_
+
+#include <ignition/physics/Implements.hh>
+
+#include <map>
+#include <memory>
+#include <string>
+
+#include "lib/src/World.hh"
+#include "lib/src/Engine.hh"
+#include "lib/src/Model.hh"
+#include "lib/src/Link.hh"
+#include "lib/src/Collision.hh"
+#include "lib/src/Shape.hh"
+
+namespace ignition {
+namespace physics {
+namespace tpeplugin {
+
+/// \brief The structs tpelib::WorldInfo,
+/// tpelib::ModelInfo, LinkInfo, and CollisionInfo are used
+/// to provide easy access to tpelib structures in the plugin library
+
+struct WorldInfo
+{
+  std::shared_ptr<tpelib::World> world;
+};
+
+struct ModelInfo
+{
+  tpelib::Model *model;
+};
+
+struct LinkInfo
+{
+  tpelib::Link *link;
+};
+
+struct CollisionInfo
+{
+  tpelib::Collision *collision;
+};
+
+class Base : public Implements3d<FeatureList<Feature>>
+{
+  public: inline Identity InitiateEngine(std::size_t /*_engineID*/) override
+  {
+    return this->GenerateIdentity(0);
+  }
+
+  public: inline std::size_t idToIndexInContainer(std::size_t _id) const
+  {
+    std::size_t index = 0;
+    if (this->childIdToParentId.find(_id) != this->childIdToParentId.end())
+    {
+      auto containerId = this->childIdToParentId.at(_id);
+      for (const auto &pair : this->childIdToParentId)
+      {
+        if (pair.first == _id && pair.second == containerId)
+        {
+          return index;
+        }
+        else if (pair.second == containerId)
+        {
+          ++index;
+        }
+      }
+    }
+    // return invalid index if not found in id map
+    return -1;
+  }
+
+  public: inline std::size_t indexInContainerToId(
+    const std::size_t _containerId, const std::size_t _index) const
+  {
+    std::size_t counter = 0;
+    auto it = this->childIdToParentId.begin();
+
+    while (counter <= _index && it != this->childIdToParentId.end())
+    {
+      if (it->second == _containerId && counter == _index)
+      {
+        return it->first;
+      }
+      else if (it->second == _containerId)
+      {
+        ++counter;
+      }
+      ++it;
+    }
+    // return invalid id if entity not found
+    return -1;
+  }
+
+
+  public: inline Identity AddWorld(std::shared_ptr<tpelib::World> _world)
+  {
+    size_t worldId = _world->GetId();
+    auto worldPtr = std::make_shared<WorldInfo>();
+    worldPtr->world = _world;
+    this->worlds.insert({worldId, worldPtr});
+    this->childIdToParentId.insert({worldId, -1});
+    return this->GenerateIdentity(worldId, worldPtr);
+  }
+
+  public: inline Identity AddModel(std::size_t _worldId, tpelib::Model &_model)
+  {
+    auto modelPtr = std::make_shared<ModelInfo>();
+    modelPtr->model = &_model;
+    size_t modelId = _model.GetId();
+    this->models.insert({modelId, modelPtr});
+    // keep track of model's corresponding world 
+    this->childIdToParentId.insert({modelId, _worldId});
+
+    return this->GenerateIdentity(modelId, modelPtr);
+  }
+
+  public: inline Identity AddLink(std::size_t _modelId, tpelib::Link &_link)
+  {
+    auto linkPtr = std::make_shared<LinkInfo>();
+    linkPtr->link = &_link;
+    size_t linkId = _link.GetId();
+    this->links.insert({linkId, linkPtr});
+    // keep track of link's corresponding model
+    this->childIdToParentId.insert({linkId, _modelId});
+
+    return this->GenerateIdentity(linkId, linkPtr);
+  }
+
+  public: inline Identity AddCollision(std::size_t _linkId,
+    tpelib::Collision &_collision)
+  {
+    auto collisionPtr = std::make_shared<CollisionInfo>();
+    collisionPtr->collision = &_collision;
+    size_t collisionId = _collision.GetId();
+    this->collisions.insert({collisionId, collisionPtr});
+    // keep track of collision's corresponding link
+    this->childIdToParentId.insert({collisionId, _linkId});
+
+    return this->GenerateIdentity(collisionId, collisionPtr);
+  }
+
+  public: std::map<std::size_t, std::shared_ptr<WorldInfo>> worlds;
+  public: std::map<std::size_t, std::shared_ptr<ModelInfo>> models;
+  public: std::map<std::size_t, std::shared_ptr<LinkInfo>> links;
+  public: std::map<std::size_t, std::shared_ptr<CollisionInfo>> collisions;
+  public: std::map<std::size_t, std::size_t> childIdToParentId;
+};
+
+}
+}
+}
+
+#endif