Merge pull request #163 from diegoferigo/refactor/new_gazebo_simulator

Refactor GazeboSimulator class

bindings/CMakeLists.txt

@@ -42,7 +42,7 @@ target_link_libraries(${swig_name} PUBLIC
  GazeboEnvironment
  ECMSingleton
  RobotSingleton
- GazeboWrapper
+ GazeboSimulator
  ScenarioGazebo
  ${PYTHON_LIBRARIES})
 

bindings/gympp_bindings.i

@@ -7,10 +7,10 @@
 #include "gympp/base/Robot.h"
 #include "gympp/base/Space.h"
 #include "gympp/gazebo/GazeboEnvironment.h"
-#include "gympp/gazebo/GazeboWrapper.h"
 #include "gympp/gazebo/GymFactory.h"
 #include "gympp/gazebo/Metadata.h"
 #include "gympp/gazebo/RobotSingleton.h"
+#include "scenario/gazebo/GazeboSimulator.h"
 #include "scenario/gazebo/Joint.h"
 #include "scenario/gazebo/Link.h"
 #include "scenario/gazebo/Model.h"
@@ -67,14 +67,14 @@
 %include "gympp/base/Space.h"
 
 %shared_ptr(gympp::base::Environment)
-%shared_ptr(gympp::gazebo::GazeboWrapper)
+%shared_ptr(scenario::gazebo::GazeboSimulator)
 %shared_ptr(gympp::gazebo::GazeboEnvironment)
 %include "ignition/common/SingletonT.hh"
 %ignore ignition::common::SingletonT<gympp::gazebo::GymFactory>::myself;
 %template(GymFactorySingleton) ignition::common::SingletonT<gympp::gazebo::GymFactory>;
 
 %include "gympp/base/Environment.h"
-%include "gympp/gazebo/GazeboWrapper.h"
+%include "scenario/gazebo/GazeboSimulator.h"
 %include "gympp/gazebo/GazeboEnvironment.h"
 
 %extend gympp::base::Robot {
@@ -101,8 +101,8 @@
  return std::dynamic_pointer_cast<gympp::gazebo::GazeboEnvironment>(env);
  }
 
- std::shared_ptr<gympp::gazebo::GazeboWrapper> envToGazeboWrapper(gympp::base::EnvironmentPtr env) {
- return std::dynamic_pointer_cast<gympp::gazebo::GazeboWrapper>(env);
+ std::shared_ptr<scenario::gazebo::GazeboSimulator> envToGazeboWrapper(gympp::base::EnvironmentPtr env) {
+ return std::dynamic_pointer_cast<scenario::gazebo::GazeboSimulator>(env);
  }
 %}
 

cpp/gympp/gazebo/CMakeLists.txt

@@ -60,7 +60,7 @@ add_library(GazeboEnvironment
 target_link_libraries(GazeboEnvironment
  PUBLIC
  gympp
- GazeboWrapper
+ GazeboSimulator
  PRIVATE
  Task
  TaskSingleton
@@ -88,6 +88,7 @@ add_library(GymFactory
 target_link_libraries(GymFactory
  PUBLIC
  gympp
+ GazeboSimulator
  GazeboEnvironment
  ignition-gazebo3::core)
 

cpp/gympp/gazebo/GazeboEnvironment.cpp

@@ -21,6 +21,7 @@
 #include <ostream>
 
 using namespace gympp::gazebo;
+using namespace scenario::gazebo;
 
 class GazeboEnvironment::Impl
 {
@@ -29,24 +30,20 @@ class GazeboEnvironment::Impl
  gympp::base::Task* task = nullptr;
 
  PluginData pluginData;
- gympp::gazebo::ModelInitData modelData;
+ ModelInitData modelData;
 };
 
-// ====================
-// IGNITION ENVIRONMENT
-// ====================
-
 bool GazeboEnvironment::initializeSimulation()
 {
  // If the server is already running it means that the simulation has been already initialized
- if (GazeboWrapper::initialized()) {
+ if (GazeboSimulator::initialized()) {
  return true;
  }
 
  gymppDebug << "Initializing the simulation" << std::endl;
 
  // Initialize gazebo and load the world file
- if (!GazeboWrapper::initialize()) {
+ if (!GazeboSimulator::initialize()) {
  gymppError << "Failed to either initialize gazebo or gather the server" << std::endl;
  return false;
  }
@@ -74,7 +71,7 @@ bool GazeboEnvironment::initializeSimulation()
  pImpl->modelData.modelName = prefix + "::" + desiredModelNameWithoutPrefix;
 
  // Insert the model in the world
- if (!insertModel(pImpl->modelData, pImpl->pluginData)) {
+ if (!GazeboSimulator::insertModel(pImpl->modelData, pImpl->pluginData)) {
  gymppError << "Failed to insert the model while resetting the environment" << std::endl;
  return false;
  }
@@ -93,7 +90,7 @@ gympp::base::Task* GazeboEnvironment::getTask()
  return pImpl->task;
 }
 
-void GazeboEnvironment::storeModelData(const gympp::gazebo::ModelInitData& modelData)
+void GazeboEnvironment::storeModelData(const scenario::gazebo::ModelInitData& modelData)
 {
  pImpl->modelData = modelData;
 }
@@ -109,9 +106,9 @@ GazeboEnvironment::GazeboEnvironment(const ActionSpacePtr aSpace,
  const double realTimeFactor,
  const double physicsUpdateRate)
  : Environment(aSpace, oSpace)
- , GazeboWrapper(static_cast<unsigned>(physicsUpdateRate / agentUpdateRate),
- realTimeFactor,
- physicsUpdateRate)
+ , GazeboSimulator(static_cast<unsigned>(physicsUpdateRate / agentUpdateRate),
+  realTimeFactor,
+  physicsUpdateRate)
  , pImpl{new GazeboEnvironment::Impl}
 {
  gymppDebug << "Configuring gazebo for an agent running at " << agentUpdateRate << " Hz"
@@ -161,7 +158,7 @@ std::optional<GazeboEnvironment::State> GazeboEnvironment::step(const Action& ac
  }
 
  // TODO rename method
- if (!GazeboWrapper::run()) {
+ if (!GazeboSimulator::run()) {
  gymppError << "Failed to step gazebo" << std::endl;
  return {};
  }
@@ -215,14 +212,14 @@ std::optional<GazeboEnvironment::Observation> GazeboEnvironment::reset()
 {
  // Check if the gazebo server is running. If reset() is executed as first method,
  // the server is initialized lazily.
- if (!initializeSimulation()) {
+ if (!this->initializeSimulation()) {
  gymppError << "Failed to initialize the simulation" << std::endl;
  assert(false);
  return {};
  }
 
  // Get the task
- auto* task = getTask();
+ auto* task = this->getTask();
  if (!task) {
  gymppError << "Failed to get the Task interface from the plugin" << std::endl;
  return {};
@@ -243,14 +240,14 @@ bool GazeboEnvironment::render(RenderMode mode)
 
  // Check if the gazebo server is running. If render() is executed as first method,
  // the server is initialized lazily.
- if (!initializeSimulation()) {
+ if (!this->initializeSimulation()) {
  gymppError << "Failed to initialize the simulation" << std::endl;
  assert(false);
  return {};
  }
 
  if (mode == RenderMode::HUMAN) {
- return GazeboWrapper::gui();
+ return GazeboSimulator::gui();
  }
 
  return false;

cpp/gympp/gazebo/GymFactory.cpp

@@ -11,6 +11,7 @@
 #include "gympp/base/Space.h"
 #include "gympp/gazebo/GazeboEnvironment.h"
 #include "gympp/gazebo/Metadata.h"
+#include "scenario/gazebo/GazeboSimulator.h"
 #include "sdf/Root.hh"
 
 #include <cassert>
@@ -100,15 +101,15 @@ gympp::base::EnvironmentPtr GymFactory::make(const std::string& envName)
  }
 
  // Create the model initialization data
- gazebo::ModelInitData modelData;
+ scenario::gazebo::ModelInitData modelData;
  modelData.sdfString = root.Element()->ToString("");
  // TODO: expose position and orientation?
 
  // Store the model data in the environment
  ignGym->storeModelData(modelData);
 
  // Create the gympp plugin data
- gympp::gazebo::PluginData pluginData;
+ scenario::gazebo::PluginData pluginData;
  pluginData.libName = md.libraryName;
  pluginData.className = md.className;
 

cpp/gympp/gazebo/include/gympp/gazebo/GazeboEnvironment.h

@@ -10,7 +10,7 @@
 #define GYMPP_GAZEBO_GAZEBOENVIRONMENT
 
 #include "gympp/base/Environment.h"
-#include "gympp/gazebo/GazeboWrapper.h"
+#include "scenario/gazebo/GazeboSimulator.h"
 
 #include <memory>
 #include <optional>
@@ -28,7 +28,7 @@ namespace gympp {
 
 class gympp::gazebo::GazeboEnvironment
  : public gympp::base::Environment
- , public gympp::gazebo::GazeboWrapper
+ , public scenario::gazebo::GazeboSimulator
  , public std::enable_shared_from_this<gympp::base::Environment>
 {
 private:
@@ -39,8 +39,8 @@ class gympp::gazebo::GazeboEnvironment
  friend class gympp::gazebo::GymFactory;
  bool initializeSimulation();
  void storeSDFModelFile(const std::string& modelSDF);
- void storeModelData(const gympp::gazebo::ModelInitData& modelData);
- void storePluginData(const gympp::gazebo::PluginData& pluginData);
+ void storeModelData(const scenario::gazebo::ModelInitData& modelData);
+ void storePluginData(const scenario::gazebo::PluginData& pluginData);
 
 public:
  using Environment = gympp::base::Environment;

cpp/gympp/gazebo/include/gympp/gazebo/Metadata.h

@@ -11,6 +11,7 @@
 
 #include "gympp/base/Log.h"
 #include "gympp/base/Space.h"
+#include "scenario/gazebo/GazeboSimulator.h"
 
 #include <string>
 #include <vector>
@@ -108,7 +109,7 @@ class gympp::gazebo::PluginMetadata
  std::string worldFileName;
 
  double agentRate;
- gazebo::PhysicsData physicsData;
+ scenario::gazebo::PhysicsData physicsData;
 
  SpaceMetadata actionSpace;
  SpaceMetadata observationSpace;
@@ -120,7 +121,7 @@ class gympp::gazebo::PluginMetadata
  inline std::string getModelFileName() const { return modelFileName; }
  inline std::string getWorldFileName() const { return worldFileName; }
  inline double getAgentRate() const { return agentRate; }
- inline gazebo::PhysicsData getPhysicsData() const { return physicsData; }
+ inline scenario::gazebo::PhysicsData getPhysicsData() const { return physicsData; }
  inline SpaceMetadata getActionSpaceMetadata() const { return actionSpace; }
  inline SpaceMetadata getObservationSpaceMetadata() const { return observationSpace; }
 
@@ -152,7 +153,7 @@ class gympp::gazebo::PluginMetadata
 
  inline void setAgentRate(const double agentRate) { this->agentRate = agentRate; }
 
- inline void setPhysicsData(const gazebo::PhysicsData& physicsData)
+ inline void setPhysicsData(const scenario::gazebo::PhysicsData& physicsData)
  {
  this->physicsData = physicsData;
  }

cpp/gympp/plugins/include/gympp/plugins/PluginDatabase.h

@@ -11,9 +11,9 @@
 
 #include "gympp/base/Log.h"
 #include "gympp/base/Space.h"
-#include "gympp/gazebo/GazeboWrapper.h"
 #include "gympp/gazebo/GymFactory.h"
 #include "gympp/gazebo/Metadata.h"
+#include "scenario/gazebo/GazeboSimulator.h"
 
 class GymppPluginRegistrator_CartPole
 {
@@ -48,7 +48,7 @@ class GymppPluginRegistrator_CartPole
  cartPoleMetadata.setActionSpaceMetadata(actionSpaceMetadata);
  cartPoleMetadata.setObservationSpaceMetadata(observationSpaceMetadata);
 
- gympp::gazebo::PhysicsData physicsData;
+ scenario::gazebo::PhysicsData physicsData;
  physicsData.rtf = 1E9;
  physicsData.maxStepSize = 0.001;
  cartPoleMetadata.setPhysicsData(physicsData);

cpp/scenario/gazebo/CMakeLists.txt

@@ -68,6 +68,7 @@ set(SCENARIO_PUBLIC_HDRS
  include/scenario/gazebo/Link.h
  include/scenario/gazebo/Log.h
  include/scenario/gazebo/utils.h
+ include/scenario/gazebo/signals.h
  include/scenario/gazebo/exceptions.h)
 
 add_library(ScenarioGazebo
@@ -78,6 +79,7 @@ add_library(ScenarioGazebo
  src/Joint.cpp
  src/Link.cpp
  src/utils.cpp
+ src/signals.cpp
  src/helpers.cpp)
 
 target_include_directories(ScenarioGazebo PUBLIC
@@ -92,6 +94,29 @@ target_link_libraries(ScenarioGazebo
 set_target_properties(ScenarioGazebo PROPERTIES
  PUBLIC_HEADER "${SCENARIO_PUBLIC_HDRS}")
 
+# ===============
+# GazeboSimulator
+# ===============
+
+add_library(GazeboSimulator
+ include/scenario/gazebo/GazeboSimulator.h
+ src/GazeboSimulator.cpp)
+
+target_link_libraries(GazeboSimulator
+ PRIVATE
+ ECMSingleton
+ ScenarioGazebo
+ ExtraComponents
+ ignition-gazebo3::core
+ tiny-process-library)
+
+set_target_properties(GazeboSimulator PROPERTIES
+ PUBLIC_HEADER include/scenario/gazebo/GazeboSimulator.h)
+
+target_include_directories(GazeboSimulator PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
 # ===================
 # Install the targets
 # ===================
@@ -107,7 +132,7 @@ if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI")
  PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/scenario/gazebo/components)
  install(
  TARGETS
- ScenarioGazebo
+ ScenarioGazebo GazeboSimulator
  EXPORT scenario
  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}