merged from main

Signed-off-by: Louise Poubel <[email protected]>

CMakeLists.txt

@@ -38,6 +38,12 @@ endif()
 # Search for project-specific dependencies
 #============================================================================
 
+# This option is needed to use the PROTOBUF_GENERATE_CPP
+# in case protobuf is found with the CMake config files
+# It needs to be set before any find_package(...) call 
+# as protobuf could be find transitively by any dependency
+set(protobuf_MODULE_COMPATIBLE TRUE)
+
 ign_find_package(sdformat12 REQUIRED)
 set(SDF_VER ${sdformat12_VERSION_MAJOR})
 
@@ -136,8 +142,6 @@ set(IGN_UTILS_VER ${ignition-utils1_VERSION_MAJOR})
 
 #--------------------------------------
 # Find protobuf
-# Module is needed to use the PROTOBUF_GENERATE_CPP
-set(protobuf_MODULE_COMPATIBLE TRUE)
 set(REQ_PROTOBUF_VER 3)
 ign_find_package(IgnProtobuf
                  VERSION ${REQ_PROTOBUF_VER}

examples/standalone/custom_server/CMakeLists.txt

@@ -2,10 +2,10 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 
 if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
   find_package(ignition-gazebo6 REQUIRED)
-  set(IGN_COMMON_VER ${ignition-gazebo6_VERSION_MAJOR})
+  set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
 
   add_executable(custom_server custom_server.cc)
   target_link_libraries(custom_server
-    ignition-gazebo${IGN_COMMON_VER}::ignition-gazebo${IGN_COMMON_VER}
+    ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}
     )
 endif()

examples/standalone/gtest_setup/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR)
+
+project(GTestSetup)
+
+# Find Gazebo
+set(IGN_GAZEBO_VER 6)
+find_package(ignition-gazebo${IGN_GAZEBO_VER} REQUIRED)
+
+# Fetch and configure GTest
+include(FetchContent)
+FetchContent_Declare(
+  googletest
+  URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip
+)
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+enable_testing()
+include(Dart)
+
+# Generate tests
+foreach(TEST_TARGET
+  command_TEST
+  gravity_TEST)
+
+  add_executable(
+    ${TEST_TARGET}
+    ${TEST_TARGET}.cc
+  )
+  target_link_libraries(${TEST_TARGET}
+    gtest_main
+    ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}
+  )
+  include(GoogleTest)
+  gtest_discover_tests(${TEST_TARGET})
+endforeach()

examples/standalone/gtest_setup/README.md

@@ -0,0 +1,27 @@
+# GTest setup
+
+Example of how to setup simulation-based tests using GTest.
+
+The example contains 2 tests:
+
+* `gravity_TEST` is a minimal example to explain the basics
+* `command_TEST` has a slightly more realistic test
+
+See the comments on the source code for more explanations.
+
+## Build
+
+From the root of the repository:
+
+    cd examples/standalone/gtest_setup
+    mkdir build
+    cd build
+    cmake ..
+    make
+
+## Run tests
+
+    cd examples/standalone/gtest_setup/build
+    ./gravity_TEST
+    ./command_TEST
+

examples/standalone/gtest_setup/command.sdf

@@ -0,0 +1,26 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="test_world">
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+
+    <model name="commanded">
+      <link name="link">
+        <inertial>
+          <inertia>
+            <ixx>0.4</ixx>
+            <iyy>0.4</iyy>
+            <izz>0.4</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+      </link>
+      <plugin
+        filename="ignition-gazebo-velocity-control-system"
+        name="ignition::gazebo::systems::VelocityControl">
+      </plugin>
+    </model>
+  </world>
+</sdf>

examples/standalone/gtest_setup/command_TEST.cc

@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) 2021 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 <gtest/gtest.h>
+
+#include <ignition/msgs/twist.pb.h>
+
+#include <ignition/common/Console.hh>
+#include <ignition/gazebo/Link.hh>
+#include <ignition/gazebo/Model.hh>
+#include <ignition/gazebo/World.hh>
+#include <ignition/gazebo/Server.hh>
+#include <ignition/gazebo/Util.hh>
+#include <ignition/gazebo/TestFixture.hh>
+#include <ignition/transport/Node.hh>
+
+using namespace std::chrono_literals;
+
+//////////////////////////////////////////////////
+// Test sending a command and verify that model reacts accordingly
+TEST(ExampleTests, Command)
+{
+  // Maximum verbosity helps with debugging
+  ignition::common::Console::SetVerbosity(4);
+
+  // Instantiate test fixture
+  ignition::gazebo::TestFixture fixture("../command.sdf");
+
+  // Get the link that we'll be inspecting
+  bool configured{false};
+  ignition::gazebo::Link link;
+  fixture.OnConfigure(
+    [&link, &configured](const ignition::gazebo::Entity &_worldEntity,
+      const std::shared_ptr<const sdf::Element> &/*_sdf*/,
+      ignition::gazebo::EntityComponentManager &_ecm,
+      ignition::gazebo::EventManager &/*_eventMgr*/)
+    {
+      ignition::gazebo::World world(_worldEntity);
+
+      auto modelEntity = world.ModelByName(_ecm, "commanded");
+      EXPECT_NE(ignition::gazebo::kNullEntity, modelEntity);
+
+      auto model = ignition::gazebo::Model(modelEntity);
+
+      auto linkEntity = model.LinkByName(_ecm, "link");
+      EXPECT_NE(ignition::gazebo::kNullEntity, linkEntity);
+
+      link = ignition::gazebo::Link(linkEntity);
+      EXPECT_TRUE(link.Valid(_ecm));
+
+      // Tell Gazebo that we want to observe the link's velocity and acceleration
+      link.EnableVelocityChecks(_ecm, true);
+      link.EnableAccelerationChecks(_ecm, true);
+
+      configured = true;
+    })
+  // Configure must be set before finalize, but other callbacks can come after
+  .Finalize();
+
+  EXPECT_TRUE(configured);
+
+  // Check that link is falling due to gravity
+  int iterations{0};
+  ignition::math::Vector3d linVel;
+  ignition::math::Vector3d linAccel;
+  fixture.OnPostUpdate(
+    [&](
+      const ignition::gazebo::UpdateInfo &_info,
+      const ignition::gazebo::EntityComponentManager &_ecm)
+    {
+      linVel = link.WorldLinearVelocity(_ecm).value();
+      EXPECT_DOUBLE_EQ(0.0, linVel.Y());
+
+      linAccel = link.WorldLinearAcceleration(_ecm).value();
+      EXPECT_DOUBLE_EQ(0.0, linAccel.X());
+      EXPECT_DOUBLE_EQ(0.0, linAccel.Y());
+
+      iterations++;
+    });
+
+  int expectedIterations{10};
+  fixture.Server()->Run(true, expectedIterations, false);
+  EXPECT_EQ(expectedIterations, iterations);
+  EXPECT_DOUBLE_EQ(0.0, linVel.X());
+  EXPECT_GT(0.0, linVel.Z());
+  EXPECT_GT(0.0, linAccel.Z());
+
+  // Send velocity command
+  ignition::transport::Node node;
+
+  ignition::msgs::Twist msg;
+  auto linVelMsg = msg.mutable_linear();
+  linVelMsg->set_x(10);
+
+  auto pub = node.Advertise<ignition::msgs::Twist>("/model/commanded/cmd_vel");
+  pub.Publish(msg);
+
+  // Commands sent through transport are processed asynchronously and may
+  // take a while to execute, so we run a few iterations until it takes
+  // effect.
+  int sleep{0};
+  int maxSleep{30};
+  for (; sleep < maxSleep && linVel.X() < 0.1; ++sleep)
+  {
+    fixture.Server()->Run(true, 10, false);
+    expectedIterations+= 10;
+    std::this_thread::sleep_for(100ms);
+  }
+  EXPECT_LT(sleep, maxSleep);
+  EXPECT_EQ(expectedIterations, iterations);
+
+  EXPECT_DOUBLE_EQ(10.0, linVel.X());
+  EXPECT_GT(0.0, linVel.Z());
+  EXPECT_GT(0.0, linAccel.Z());
+}

examples/standalone/gtest_setup/gravity.sdf

@@ -0,0 +1,22 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="gravity">
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+
+    <model name="falling">
+      <link name="link">
+        <inertial>
+          <inertia>
+            <ixx>0.4</ixx>
+            <iyy>0.4</iyy>
+            <izz>0.4</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+      </link>
+    </model>
+  </world>
+</sdf>

examples/standalone/gtest_setup/gravity_TEST.cc

@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2021 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 <gtest/gtest.h>
+
+#include <ignition/common/Console.hh>
+#include <ignition/gazebo/Model.hh>
+#include <ignition/gazebo/World.hh>
+#include <ignition/gazebo/Server.hh>
+#include <ignition/gazebo/Util.hh>
+#include <ignition/gazebo/TestFixture.hh>
+
+//////////////////////////////////////////////////
+// Test that an object falls due to gravity
+TEST(ExampleTests, Gravity)
+{
+  // Maximum verbosity helps with debugging
+  ignition::common::Console::SetVerbosity(4);
+
+  // Instantiate test fixture. It starts a server and provides hooks that we'll
+  // use to inspect the running simulation.
+  ignition::gazebo::TestFixture fixture("../gravity.sdf");
+
+  int iterations{0};
+  ignition::gazebo::Entity modelEntity;
+  ignition::math::Vector3d gravity;
+
+  fixture.
+  // Use configure callback to get values at startup
+  OnConfigure(
+    [&modelEntity, &gravity](const ignition::gazebo::Entity &_worldEntity,
+      const std::shared_ptr<const sdf::Element> &/*_sdf*/,
+      ignition::gazebo::EntityComponentManager &_ecm,
+      ignition::gazebo::EventManager &/*_eventMgr*/)
+    {
+      // Get gravity
+      ignition::gazebo::World world(_worldEntity);
+      gravity = world.Gravity(_ecm).value();
+
+      // Get falling entity
+      modelEntity = world.ModelByName(_ecm, "falling");
+      EXPECT_NE(ignition::gazebo::kNullEntity, modelEntity);
+    }).
+  // Use post-update callback to get values at the end of every iteration
+  OnPostUpdate(
+    [&iterations, &modelEntity, &gravity](
+      const ignition::gazebo::UpdateInfo &_info,
+      const ignition::gazebo::EntityComponentManager &_ecm)
+    {
+      // Inspect all model poses
+      auto pose = ignition::gazebo::worldPose(modelEntity, _ecm);
+
+      EXPECT_DOUBLE_EQ(0.0, pose.Pos().X());
+      EXPECT_DOUBLE_EQ(0.0, pose.Pos().Y());
+
+      // Check that model is falling due to gravity
+      // -g * t^2 / 2
+      auto time = std::chrono::duration<double>(_info.simTime).count();
+      EXPECT_NEAR(gravity.Z() * time * time * 0.5, pose.Pos().Z(), 1e-2);
+
+      iterations++;
+    }).
+  // The moment we finalize, the configure callback is called
+  Finalize();
+
+  // Setup simulation server, this will call the post-update callbacks.
+  // It also calls pre-update and update callbacks if those are being used.
+  fixture.Server()->Run(true, 1000, false);
+
+  // Verify that the post update function was called 1000 times
+  EXPECT_EQ(1000, iterations);
+}