[Citadel] Add all features to tpeplugin

tpe/lib/src/Model.cc

@@ -46,6 +46,14 @@ Entity &Model::AddLink()
   return *it->second.get();
 }
 
+//////////////////////////////////////////////////
+Entity &Model::GetCanonicalLink()
+{
+  // return first link as canonical link
+  return *this->GetChildren().begin()->second;
+}
+
+
 //////////////////////////////////////////////////
 void Model::SetLinearVelocity(const math::Vector3d _velocity)
 {

tpe/lib/src/Model.hh

@@ -47,6 +47,10 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
   /// \return Newly created Link
   public: Entity &AddLink();
 
+  /// \brief Get the canonical link of model
+  /// \return Entity the canonical (first) link
+  public: Entity &GetCanonicalLink();
+
   /// \brief Set the linear velocity of model
   /// \param[in] _velocity linear velocity
   public: void SetLinearVelocity(const math::Vector3d _velocity);

tpe/lib/src/Shape.cc

@@ -84,6 +84,12 @@ void BoxShape::SetSize(const math::Vector3d &_size)
   this->dirty = true;
 }
 
+//////////////////////////////////////////////////
+math::Vector3d BoxShape::GetSize()
+{
+  return this->size;
+}
+
 //////////////////////////////////////////////////
 void BoxShape::UpdateBoundingBox()
 {

tpe/lib/src/Shape_TEST.cc

@@ -37,6 +37,7 @@ TEST(Shape, BoxShape)
 
   math::Vector3d size(1.2, 3.6, 5.8);
   shape.SetSize(size);
+  EXPECT_EQ(size, shape.GetSize());
   math::AxisAlignedBox bbox = shape.GetBoundingBox();
   EXPECT_EQ(math::Vector3d::Zero, bbox.Center());
   EXPECT_EQ(size, bbox.Size());

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,133 @@
+/*
+ * 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::ModelInfo,
+/// tpelib::LinkInfo, JointInfo, and ShapeInfo are used
+/// for two reasons:
+/// 1) Holding extra information such as the name or offset
+///    that will be different from the underlying engine
+/// 2) Wrap shared pointers to DART entities. Since these shared pointers (eg.
+///    dart::dynamics::BodyNodePtr) are different from std::shared_ptr, we
+///    cannot use them directly as parameters to GenerateIdentity. Instead we
+///    create a std::shared_ptr of the struct that wraps the corresponding DART
+///    shared pointer.
+
+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 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});
+    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

tpe/plugin/src/CustomFeatures.cc

@@ -0,0 +1,29 @@
+/*
+ * 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.
+ *
+*/
+
+#include "CustomFeatures.hh"
+
+using namespace ignition;
+using namespace physics;
+using namespace tpeplugin;
+
+/////////////////////////////////////////////////
+std::shared_ptr<tpelib::World> CustomFeatures::GetTpeLibWorld(
+  const Identity &_worldID)
+{
+  return this->worlds.at(_worldID)->world;
+}

tpe/plugin/src/CustomFeatures.hh

@@ -0,0 +1,48 @@
+/*
+ * 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_CUSTOMFEATURES_HH
+#define IGNITION_PHYSICS_TPE_PLUGIN_SRC_CUSTOMFEATURES_HH
+
+#include <memory>
+
+#include <ignition/physics/Implements.hh>
+
+#include "Base.hh"
+#include "World.hh"
+
+namespace ignition {
+namespace physics {
+namespace tpeplugin {
+
+using CustomFeatureList = FeatureList<
+  RetrieveWorld
+>;
+
+class CustomFeatures :
+  public virtual Base,
+  public virtual Implements3d<CustomFeatureList>
+{
+  public: std::shared_ptr<tpelib::World> GetTpeLibWorld(
+    const Identity &_worldID) override;
+};
+
+}
+}
+}
+
+#endif