-
Notifications
You must be signed in to change notification settings - Fork 276
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Louise Poubel <[email protected]>
- Loading branch information
Showing
39 changed files
with
1,799 additions
and
88 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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()); | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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); | ||
} |
Oops, something went wrong.