diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 03b503baaf..409860ac69 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -34,6 +34,7 @@ set(tests nondefault_world.cc physics.cc physics_msgs.cc + physics_thread_safe.cc pioneer2dx.cc transport.cc server_fixture.cc diff --git a/test/integration/physics_thread_safe.cc b/test/integration/physics_thread_safe.cc new file mode 100644 index 0000000000..b7c0ce80d0 --- /dev/null +++ b/test/integration/physics_thread_safe.cc @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2012-2013 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 + +#include "gazebo/msgs/msgs.hh" +#include "gazebo/physics/physics.hh" +#include "gazebo/transport/transport.hh" +#include "ServerFixture.hh" +#include "helper_physics_generator.hh" + +#define PHYSICS_TOL 1e-2 +using namespace gazebo; + +class PhysicsThreadSafeTest : public ServerFixture, + public testing::WithParamInterface +{ + public: void LinkGet(const std::string &_physicsEngine); +}; + +///////////////////////////////////////////////// +void PhysicsThreadSafeTest::LinkGet(const std::string &_physicsEngine) +{ + Load("worlds/revolute_joint_test.world", true, _physicsEngine); + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + + physics::PhysicsEnginePtr physics = world->GetPhysicsEngine(); + ASSERT_TRUE(physics != NULL); + EXPECT_EQ(physics->GetType(), _physicsEngine); + + // Unthrottle the update rate + physics->SetRealTimeUpdateRate(0); + + std::string modelName = "pendulum_0deg"; + std::string linkName = "lower_link"; + + physics::ModelPtr model = world->GetModel(modelName); + ASSERT_TRUE(model != NULL); + + physics::LinkPtr link = model->GetLink(linkName); + ASSERT_TRUE(link != NULL); + + // Start the simulation + world->SetPaused(false); + + // Run for 20 seconds of sim time + while (world->GetSimTime().sec < 20) + { + // Call these functions repeatedly + // Test passes if it doesn't abort early + math::Vector3 vel = link->GetWorldLinearVel(); + vel += link->GetWorldAngularVel(); + } +} + +///////////////////////////////////////////////// +TEST_P(PhysicsThreadSafeTest, LinkGet) +{ + LinkGet(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsThreadSafeTest, + PHYSICS_ENGINE_VALUES); + +///////////////////////////////////////////////// +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}