Move and update Ignition plugins to work with ScenarI/O

CMakeLists.txt

@@ -120,11 +120,11 @@ add_subdirectory(cpp/gympp/base)
 add_subdirectory(cpp/gympp/gazebo)
 add_subdirectory(cpp/gympp/plugins)
 add_subdirectory(cpp/scenario/gazebo)
+add_subdirectory(cpp/scenario/plugins)
 
 # Add the targets depending on Ignition Robotics
 if(${GYMIGNITION_USE_IGNITION})
     add_subdirectory(ignition)
-    add_subdirectory(plugins)
     add_subdirectory(gym_ignition_data)
     add_subdirectory(examples/cpp)
 

bindings/CMakeLists.txt

@@ -40,6 +40,7 @@ target_link_libraries(${swig_name} PUBLIC
     RobotSingleton
     GymFactory
     GazeboEnvironment
+    ECMSingleton
     RobotSingleton
     GazeboWrapper
     ScenarioGazebo

cpp/scenario/plugins/CMakeLists.txt

@@ -0,0 +1,27 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+# All rights reserved.
+#
+#  This project is dual licensed under LGPL v2.1+ or Apache License.
+#
+# -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -
+#
+#  This software may be modified and distributed under the terms of the
+#  GNU Lesser General Public License v2.1 or any later version.
+#
+# -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -
+#
+#  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.
+
+add_subdirectory(Physics)
+add_subdirectory(ECMProvider)
+add_subdirectory(RobotController)

cpp/scenario/plugins/ECMProvider/CMakeLists.txt

@@ -0,0 +1,84 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+# All rights reserved.
+#
+#  This project is dual licensed under LGPL v2.1+ or Apache License.
+#
+# -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -
+#
+#  This software may be modified and distributed under the terms of the
+#  GNU Lesser General Public License v2.1 or any later version.
+#
+# -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -   -
+#
+#  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.
+
+# ===========
+# ECMProvider
+# ===========
+
+add_library(ECMProvider SHARED
+    include/scenario/plugins/gazebo/ECMProvider.h
+    ECMProvider.cpp)
+
+target_link_libraries(ECMProvider
+    PUBLIC
+    ignition-gazebo3::core
+    PRIVATE
+    ECMSingleton
+    ExtraComponents)
+
+target_include_directories(ECMProvider PRIVATE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+
+# ============
+# ECMSingleton
+# ============
+
+add_library(ECMSingleton
+    include/scenario/plugins/gazebo/ECMSingleton.h
+    ECMSingleton.cpp)
+
+target_include_directories(ECMSingleton PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
+target_link_libraries(ECMSingleton
+    PUBLIC
+    ignition-gazebo3::core
+    PRIVATE
+    ScenarioGazebo)
+
+if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI")
+    set_target_properties(ECMSingleton PROPERTIES
+        PUBLIC_HEADER include/scenario/plugins/gazebo/ECMSingleton.h)
+endif()
+
+# ===================
+# Install the targets
+# ===================
+
+if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI")
+    install(
+        TARGETS ECMProvider
+        EXPORT scenario
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/scenario/plugins
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}/scenario/plugins
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/scenario/plugins)
+    install(
+        TARGETS ECMSingleton
+        EXPORT scenario
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+        PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/scenario/plugins/gazebo)
+endif()

cpp/scenario/plugins/ECMProvider/ECMProvider.cpp

@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This project is dual licensed under LGPL v2.1+ or Apache License.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * This software may be modified and distributed under the terms of the
+ * GNU Lesser General Public License v2.1 or any later version.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * 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 "scenario/plugins/gazebo/ECMProvider.h"
+#include "scenario/gazebo/Log.h"
+#include "scenario/plugins/gazebo/ECMSingleton.h"
+
+#include <ignition/gazebo/Entity.hh>
+#include <ignition/plugin/Register.hh>
+
+using namespace scenario::plugins::gazebo;
+
+class ECMProvider::Impl
+{
+public:
+};
+
+ECMProvider::ECMProvider()
+    : System()
+    , pImpl{std::make_unique<Impl>()}
+{}
+
+ECMProvider::~ECMProvider()
+{
+    gymppDebug << "Destroying the ECMProvider" << std::endl;
+};
+
+void ECMProvider::Configure(const ignition::gazebo::Entity& entity,
+                            const std::shared_ptr<const sdf::Element>& /*sdf*/,
+                            ignition::gazebo::EntityComponentManager& ecm,
+                            ignition::gazebo::EventManager& eventMgr)
+{
+    if (ECMSingleton::Instance().valid()) {
+        gymppWarning << "The ECM singleton has been already configured"
+                     << std::endl;
+        return;
+    }
+
+    gymppDebug << "Storing ECM resources in the singleton" << std::endl;
+
+    if (!ECMSingleton::Instance().storePtrs(&ecm, &eventMgr)) {
+        gymppError << "Failed to store ECM in the singleton for entity ["
+                   << entity << "]" << std::endl;
+        return;
+    }
+}
+
+IGNITION_ADD_PLUGIN(scenario::plugins::gazebo::ECMProvider,
+                    scenario::plugins::gazebo::ECMProvider::System,
+                    scenario::plugins::gazebo::ECMProvider::ISystemConfigure)

cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp

@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This project is dual licensed under LGPL v2.1+ or Apache License.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * This software may be modified and distributed under the terms of the
+ * GNU Lesser General Public License v2.1 or any later version.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * 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 "scenario/plugins/gazebo/ECMSingleton.h"
+#include "scenario/gazebo/Log.h"
+
+#include <atomic>
+#include <ostream>
+#include <string>
+
+using namespace scenario::plugins::gazebo;
+
+class ECMSingleton::Impl
+{
+public:
+    std::atomic<ignition::gazebo::EventManager*> eventMgr = nullptr;
+    std::atomic<ignition::gazebo::EntityComponentManager*> ecm = nullptr;
+};
+
+ECMSingleton::ECMSingleton()
+    : pImpl{new Impl()}
+{}
+
+ECMSingleton& ECMSingleton::Instance()
+{
+    static ECMSingleton instance;
+    return instance;
+}
+
+bool ECMSingleton::valid() const
+{
+    return pImpl->ecm && pImpl->eventMgr;
+}
+
+ignition::gazebo::EventManager* ECMSingleton::getEventManager() const
+{
+    if (!this->valid()) {
+        gymppError << "The pointers are not valid" << std::endl;
+        return nullptr;
+    }
+
+    return pImpl->eventMgr;
+}
+
+bool ECMSingleton::storePtrs(ignition::gazebo::EntityComponentManager* ecm,
+                             ignition::gazebo::EventManager* eventMgr)
+{
+    if (!ecm || !eventMgr) {
+        gymppError << "The pointer to the ECM or EventManager is not valid"
+                   << std::endl;
+        return false;
+    }
+
+    pImpl->ecm = ecm;
+    pImpl->eventMgr = eventMgr;
+
+    return true;
+}
+
+ignition::gazebo::EntityComponentManager* ECMSingleton::getECM() const
+{
+
+    if (!this->valid()) {
+        gymppError << "The pointers are not valid" << std::endl;
+        return nullptr;
+    }
+
+    return pImpl->ecm;
+}
+
+void ECMSingleton::clean()
+{
+    pImpl->ecm = nullptr;
+    pImpl->eventMgr = nullptr;
+}

cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMProvider.h

@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This project is dual licensed under LGPL v2.1+ or Apache License.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * This software may be modified and distributed under the terms of the
+ * GNU Lesser General Public License v2.1 or any later version.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * 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 SCENARIO_PLUGINS_GAZEBO_ECMPROVIDER
+#define SCENARIO_PLUGINS_GAZEBO_ECMPROVIDER
+
+#include <ignition/gazebo/Entity.hh>
+#include <ignition/gazebo/EntityComponentManager.hh>
+#include <ignition/gazebo/EventManager.hh>
+#include <ignition/gazebo/System.hh>
+#include <sdf/Element.hh>
+
+#include <memory>
+
+namespace scenario {
+    namespace plugins {
+        namespace gazebo {
+            class ECMProvider;
+        } // namespace gazebo
+    } // namespace plugins
+} // namespace scenario
+
+class scenario::plugins::gazebo::ECMProvider final
+    : public ignition::gazebo::System
+    , public ignition::gazebo::ISystemConfigure
+{
+public:
+    ECMProvider();
+    ~ECMProvider() override;
+
+    void Configure(const ignition::gazebo::Entity& entity,
+                   const std::shared_ptr<const sdf::Element>& sdf,
+                   ignition::gazebo::EntityComponentManager& ecm,
+                   ignition::gazebo::EventManager& eventMgr) override;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pImpl = nullptr;
+};
+
+#endif // SCENARIO_PLUGINS_GAZEBO_ECMPROVIDER