Added dynamic generation and loading sdf file into gazebo functionality.

CMakeLists.txt

@@ -23,7 +23,9 @@ add_library(gtest_main STATIC gtest/src/gtest_main.cc)
 target_link_libraries(gtest_main gtest tbb)
 
 include (${PROJECT_SOURCE_DIR}/tools/TestMacro.cmake)
+set(WORLDS_DIR_PATH "${PROJECT_SOURCE_DIR}/worlds")
 set(TEST_TYPE "BENCHMARK")
+configure_file(${PROJECT_SOURCE_DIR}/PathConfig.h.in PathConfig.h)
 
 # Boxes tests
 set(BOXES_TEST_FILES

PathConfig.h.in

@@ -0,0 +1 @@
+#define WORLDS_DIR_PATH "@WORLDS_DIR_PATH@"

boxes.cc

@@ -13,16 +13,20 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *
-*/
+ */
 #include <string>
 
 #include <ignition/math/Pose3.hh>
 #include <ignition/math/Quaternion.hh>
 #include <ignition/math/Vector3.hh>
 
+#include "PathConfig.h"
+#include "boxes.hh"
+#include "gazebo/common/common.hh"
 #include "gazebo/msgs/msgs.hh"
 #include "gazebo/physics/physics.hh"
-#include "boxes.hh"
+#include <boost/format.hpp>
+#include <sstream>
 
 using namespace gazebo;
 using namespace benchmark;
@@ -31,14 +35,30 @@ using namespace benchmark;
 // Boxes:
 // Spawn a single box and record accuracy for momentum and enery
 // conservation
-void BoxesTest::Boxes(const std::string &_physicsEngine
-                    , double _dt
-                    , int _modelCount
-                    , bool _collision
-                    , bool _complex)
-{
+void BoxesTest::Boxes(const std::string &_physicsEngine, double _dt,
+                      int _modelCount, bool _collision, bool _complex) {
+  ASSERT_GT(_modelCount, 0);
+  std::string world_erb_path =
+      boost::str(boost::format("%1%/boxes.world.erb") % WORLDS_DIR_PATH);
+
+  std::string world_path = boost::str(
+      boost::format(
+          "%1%/%2%/"
+          "boxes_collision%3%_complex%4%_dt%5$.0e_modelCount%6%.world") %
+      WORLDS_DIR_PATH % TEST_NAME % _collision % _complex % _dt % _modelCount);
+
+  std::string command = boost::str(
+      boost::format(
+          "erb collision=%1% complex=%2% dt=%3% modelCount=%4% %5% > %6%") %
+      _collision % _complex % _dt % _modelCount % world_erb_path % world_path);
+
+  // creating model with desired configuration
+  auto model_check = system(command.c_str());
+  // checking if model is created
+  ASSERT_EQ(model_check, 0);
+
   // Load a blank world (no ground plane)
-  Load("worlds/blank.world", true, _physicsEngine);
+  Load(world_path, true, _physicsEngine);
   physics::WorldPtr world = physics::get_world("default");
   ASSERT_NE(world, nullptr);
 
@@ -47,39 +67,19 @@ void BoxesTest::Boxes(const std::string &_physicsEngine
   ASSERT_NE(physics, nullptr);
   ASSERT_EQ(physics->GetType(), _physicsEngine);
 
-  // get gravity value
-  if (!_complex)
-  {
-    world->SetGravity(ignition::math::Vector3d::Zero);
-  }
   ignition::math::Vector3d g = world->Gravity();
 
-  // Box size
-  const double dx = 0.1;
-  const double dy = 0.4;
-  const double dz = 0.9;
-  const double mass = 10.0;
-  // expected inertia matrix, recompute if the above change
   const double Ixx = 0.80833333;
   const double Iyy = 0.68333333;
   const double Izz = 0.14166667;
-  const ignition::math::Matrix3d I0(Ixx, 0.0, 0.0
-                                  , 0.0, Iyy, 0.0
-                                  , 0.0, 0.0, Izz);
-
-  // Create box with inertia based on box of uniform density
-  msgs::Model msgModel;
-  msgs::AddBoxLink(msgModel, mass, ignition::math::Vector3d(dx, dy, dz));
-  if (!_collision)
-  {
-    // Test without collision shapes.
-    msgModel.mutable_link(0)->clear_collision();
-  }
+  const ignition::math::Matrix3d I0(Ixx, 0.0, 0.0, 0.0, Iyy, 0.0, 0.0, 0.0,
+                                    Izz);
 
-  // spawn multiple boxes
-  // compute error statistics only on the last box
-  ASSERT_GT(_modelCount, 0);
-  physics::ModelPtr model;
+  // physics::ModelPtr model;
+  std::size_t model_count = world->ModelCount();
+  ASSERT_EQ(model_count, _modelCount);
+
+  auto models = world->Models();
   physics::LinkPtr link;
 
   // initial linear velocity in global frame
@@ -91,46 +91,31 @@ void BoxesTest::Boxes(const std::string &_physicsEngine
   // initial energy value
   double E0;
 
-  if (!_complex)
-  {
+  if (!_complex) {
     v0.Set(-0.9, 0.4, 0.1);
     // Use angular velocity with one non-zero component
     // to ensure linear angular trajectory
     w0.Set(0.5, 0, 0);
     E0 = 5.001041625;
-  }
-  else
-  {
+  } else {
     v0.Set(-2.0, 2.0, 8.0);
     // Since Ixx > Iyy > Izz,
     // angular velocity with large y component
     // will cause gyroscopic tumbling
     w0.Set(0.1, 5.0, 0.1);
     E0 = 368.54641249999997;
   }
+  // adding a small delay (waiting for the model to load properly)
+  common::Time::MSleep(50);
 
-  for (int i = 0; i < _modelCount; ++i)
-  {
-    // give models unique names
-    msgModel.set_name(this->GetUniqueString("model"));
-    // give models unique positions
-    msgs::Set(msgModel.mutable_pose()->mutable_position(),
-              ignition::math::Vector3d(0.0, dz*2*i, 0.0));
-
-    model = this->SpawnModel(msgModel);
-    ASSERT_NE(model, nullptr);
-
+  for (auto model : models) {
     link = model->GetLink();
     ASSERT_NE(link, nullptr);
-
-    // Set initial conditions
-    link->SetLinearVel(v0);
-    link->SetAngularVel(w0);
+    ASSERT_EQ(v0, link->WorldCoGLinearVel());
+    ASSERT_EQ(w0, link->WorldAngularVel());
+    ASSERT_EQ(I0, link->GetInertial()->MOI(link->GetInertial()->Pose()));
+    ASSERT_NEAR(link->GetWorldEnergy(), E0, 1e-5);
   }
-  ASSERT_EQ(v0, link->WorldCoGLinearVel());
-  ASSERT_EQ(w0, link->WorldAngularVel());
-  ASSERT_EQ(I0, link->GetInertial()->MOI());
-  ASSERT_NEAR(link->GetWorldEnergy(), E0, 1e-6);
 
   // initial time
   common::Time t0 = world->SimTime();
@@ -143,9 +128,6 @@ void BoxesTest::Boxes(const std::string &_physicsEngine
   ASSERT_EQ(H0, ignition::math::Vector3d(Ixx, Iyy, Izz) * w0);
   double H0mag = H0.Length();
 
-  // change step size after setting initial conditions
-  // since simbody requires a time step
-  physics->SetMaxStepSize(_dt);
   const double simDuration = 10.0;
   int steps = ceil(simDuration / _dt);
 
@@ -167,28 +149,26 @@ void BoxesTest::Boxes(const std::string &_physicsEngine
   // unthrottle update rate
   physics->SetRealTimeUpdateRate(0.0);
   common::Time startTime = common::Time::GetWallTime();
-  for (int i = 0; i < steps; ++i)
-  {
+  for (int i = 0; i < steps; ++i) {
     world->Step(1);
 
     // current time
     double t = (world->SimTime() - t0).Double();
 
     // linear velocity error
     ignition::math::Vector3d v = link->WorldCoGLinearVel();
-    linearVelocityError.InsertData(v - (v0 + g*t));
+    linearVelocityError.InsertData(v - (v0 + g * t));
 
     // linear position error
     ignition::math::Vector3d p = link->WorldInertialPose().Pos();
-    linearPositionError.InsertData(p - (p0 + v0 * t + 0.5*g*t*t));
+    linearPositionError.InsertData(p - (p0 + v0 * t + 0.5 * g * t * t));
 
     // angular momentum error
     ignition::math::Vector3d H = link->WorldAngularMomentum();
     angularMomentumError.InsertData((H - H0) / H0mag);
 
     // angular position error
-    if (!_complex)
-    {
+    if (!_complex) {
       ignition::math::Vector3d a = link->WorldInertialPose().Rot().Euler();
       ignition::math::Quaterniond angleTrue(w0 * t);
       angularPositionError.InsertData(a - angleTrue.Euler());
@@ -200,7 +180,7 @@ void BoxesTest::Boxes(const std::string &_physicsEngine
   common::Time elapsedTime = common::Time::GetWallTime() - startTime;
   this->Record("wallTime", elapsedTime.Double());
   common::Time simTime = (world->SimTime() - t0).Double();
-  ASSERT_NEAR(simTime.Double(), simDuration, _dt*1.1);
+  ASSERT_NEAR(simTime.Double(), simDuration, _dt * 1.1);
   this->Record("simTime", simTime.Double());
   this->Record("timeRatio", elapsedTime.Double() / simTime.Double());
 
@@ -215,27 +195,19 @@ void BoxesTest::Boxes(const std::string &_physicsEngine
 }
 
 /////////////////////////////////////////////////
-TEST_P(BoxesTest, Boxes)
-{
+TEST_P(BoxesTest, Boxes) {
   std::string physicsEngine = std::tr1::get<0>(GetParam());
-  double dt                 = std::tr1::get<1>(GetParam());
-  int modelCount            = std::tr1::get<2>(GetParam());
-  bool collision            = std::tr1::get<3>(GetParam());
-  bool isComplex            = std::tr1::get<4>(GetParam());
-  gzdbg << physicsEngine
-        << ", dt: " << dt
-        << ", modelCount: " << modelCount
-        << ", collision: " << collision
-        << ", isComplex: " << isComplex
+  double dt = std::tr1::get<1>(GetParam());
+  int modelCount = std::tr1::get<2>(GetParam());
+  bool collision = std::tr1::get<3>(GetParam());
+  bool isComplex = std::tr1::get<4>(GetParam());
+  gzdbg << physicsEngine << ", dt: " << dt << ", modelCount: " << modelCount
+        << ", collision: " << collision << ", isComplex: " << isComplex
         << std::endl;
   RecordProperty("engine", physicsEngine);
   this->Record("dt", dt);
   RecordProperty("modelCount", modelCount);
   RecordProperty("collision", collision);
   RecordProperty("isComplex", isComplex);
-  Boxes(physicsEngine
-      , dt
-      , modelCount
-      , collision
-      , isComplex);
+  Boxes(physicsEngine, dt, modelCount, collision, isComplex);
 }

boxes_model_count.cc

@@ -13,7 +13,7 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *
-*/
+ */
 #include <string.h>
 
 #include "boxes.hh"
@@ -26,43 +26,42 @@ const int g_models_min = 1;
 const int g_models_max = 105;
 const int g_models_step = 20;
 
-INSTANTIATE_TEST_CASE_P(OdeBoxes, BoxesTest,
-  ::testing::Combine(::testing::Values("ode")
-  , ::testing::Values(5.0e-4)
-  , ::testing::Range(g_models_min, g_models_max, g_models_step)
-  , ::testing::Bool()
-  , ::testing::Values(true)));
+INSTANTIATE_TEST_CASE_P(
+    OdeBoxes, BoxesTest,
+    ::testing::Combine(::testing::Values("ode"), ::testing::Values(5.0e-4),
+                       ::testing::Range(g_models_min, g_models_max,
+                                        g_models_step),
+                       ::testing::Bool(), ::testing::Values(true)));
 
 #ifdef HAVE_BULLET
-INSTANTIATE_TEST_CASE_P(BulletBoxes, BoxesTest,
-  ::testing::Combine(::testing::Values("bullet")
-  , ::testing::Values(5.0e-4)
-  , ::testing::Range(g_models_min, g_models_max, g_models_step)
-  , ::testing::Bool()
-  , ::testing::Values(true)));
+INSTANTIATE_TEST_CASE_P(
+    BulletBoxes, BoxesTest,
+    ::testing::Combine(::testing::Values("bullet"), ::testing::Values(5.0e-4),
+                       ::testing::Range(g_models_min, g_models_max,
+                                        g_models_step),
+                       ::testing::Bool(), ::testing::Values(true)));
 #endif
 
 #ifdef HAVE_SIMBODY
-INSTANTIATE_TEST_CASE_P(SimbodyBoxes, BoxesTest,
-  ::testing::Combine(::testing::Values("simbody")
-  , ::testing::Values(1.0e-3)
-  , ::testing::Range(g_models_min, g_models_max, g_models_step)
-  , ::testing::Bool()
-  , ::testing::Values(true)));
+INSTANTIATE_TEST_CASE_P(
+    SimbodyBoxes, BoxesTest,
+    ::testing::Combine(::testing::Values("simbody"), ::testing::Values(1.0e-3),
+                       ::testing::Range(g_models_min, g_models_max,
+                                        g_models_step),
+                       ::testing::Bool(), ::testing::Values(true)));
 #endif
 
 #ifdef HAVE_DART
-INSTANTIATE_TEST_CASE_P(DartBoxes, BoxesTest,
-  ::testing::Combine(::testing::Values("dart")
-  , ::testing::Values(5.0e-4)
-  , ::testing::Range(g_models_min, g_models_max, g_models_step)
-  , ::testing::Bool()
-  , ::testing::Values(true)));
+INSTANTIATE_TEST_CASE_P(
+    DartBoxes, BoxesTest,
+    ::testing::Combine(::testing::Values("dart"), ::testing::Values(5.0e-4),
+                       ::testing::Range(g_models_min, g_models_max,
+                                        g_models_step),
+                       ::testing::Bool(), ::testing::Values(true)));
 #endif
 
 /////////////////////////////////////////////////
-int main(int argc, char **argv)
-{
+int main(int argc, char **argv) {
   ::testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }

boxes_results.ipynb

@@ -184,4 +184,4 @@
  },
  "nbformat": 4,
  "nbformat_minor": 5
-}
+}

tools/TestMacro.cmake

@@ -7,23 +7,33 @@ macro (gz_build_tests)
   foreach(GTEST_SOURCE_file ${ARGN})
     string(REGEX REPLACE ".cc" "" BINARY_NAME ${GTEST_SOURCE_file})
     set(BINARY_NAME ${TEST_TYPE}_${BINARY_NAME})
+
+    # Check if the file exists
+    if (EXISTS "${WORLDS_DIR_PATH}/${BINARY_NAME}")
+          message(STATUS "${WORLDS_DIR_PATH}/${BINARY_NAME} exists!")
+    else()
+          execute_process(COMMAND mkdir ${WORLDS_DIR_PATH}/${BINARY_NAME})
+          message(STATUS "${WORLDS_DIR_PATH}/${BINARY_NAME} created")
+    endif()
     add_executable(${BINARY_NAME} ${GTEST_SOURCE_file}
                    ${GZ_BUILD_TESTS_EXTRA_EXE_SRCS})
-
+
+    target_include_directories(${BINARY_NAME} PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
     target_link_libraries(${BINARY_NAME}
       gtest
       gtest_main
       gazebo_test_fixture
       ${GAZEBO_LIBRARIES}
       ${Boost_LIBRARIES}
     )
+    target_compile_definitions(${BINARY_NAME} PRIVATE TEST_NAME="${BINARY_NAME}")
 
     add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME}
 	    --gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
 
     set(_env_vars)
     list(APPEND _env_vars "GAZEBO_MODEL_PATH=${CMAKE_SOURCE_DIR}/models:${GAZEBO_MODEL_PATH}")
-    #list(APPEND _env_vars "GAZEBO_RESOURCE_PATH=${CMAKE_SOURCE_DIR}:${GAZEBO_RESOURCE_PATH}")
+    # list(APPEND _env_vars "GAZEBO_RESOURCE_PATH=${CMAKE_SOURCE_DIR}:${GAZEBO_RESOURCE_PATH}")
     set_tests_properties(${BINARY_NAME} PROPERTIES
       TIMEOUT 240
       ENVIRONMENT "${_env_vars}"