235 lines
8.6 KiB
C++
235 lines
8.6 KiB
C++
/*
|
|
* Copyright (C) 2012 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 "gazebo/test/ServerFixture.hh"
|
|
#include "gazebo/test/helper_physics_generator.hh"
|
|
#include "gazebo/msgs/msgs.hh"
|
|
|
|
using namespace gazebo;
|
|
|
|
class PhysicsEngineTest : public ServerFixture,
|
|
public testing::WithParamInterface<const char*>
|
|
{
|
|
public: void OnPhysicsMsgResponse(ConstResponsePtr &_msg);
|
|
public: void PhysicsEngineParam(const std::string &_physicsEngine);
|
|
public: void PhysicsEngineGetParamBool(const std::string &_physicsEngine);
|
|
public: static msgs::Physics physicsPubMsg;
|
|
public: static msgs::Physics physicsResponseMsg;
|
|
};
|
|
|
|
msgs::Physics PhysicsEngineTest::physicsPubMsg;
|
|
msgs::Physics PhysicsEngineTest::physicsResponseMsg;
|
|
|
|
/////////////////////////////////////////////////
|
|
void PhysicsEngineTest::OnPhysicsMsgResponse(ConstResponsePtr &_msg)
|
|
{
|
|
if (_msg->type() == physicsPubMsg.GetTypeName())
|
|
physicsResponseMsg.ParseFromString(_msg->serialized_data());
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void PhysicsEngineTest::PhysicsEngineParam(const std::string &_physicsEngine)
|
|
{
|
|
physicsPubMsg.Clear();
|
|
physicsResponseMsg.Clear();
|
|
|
|
Load("worlds/empty.world", false, _physicsEngine);
|
|
physics::WorldPtr world = physics::get_world("default");
|
|
ASSERT_TRUE(world != NULL);
|
|
|
|
transport::NodePtr physicsNode;
|
|
physicsNode = transport::NodePtr(new transport::Node());
|
|
physicsNode->Init();
|
|
|
|
transport::PublisherPtr physicsPub
|
|
= physicsNode->Advertise<msgs::Physics>("~/physics");
|
|
transport::PublisherPtr requestPub
|
|
= physicsNode->Advertise<msgs::Request>("~/request");
|
|
transport::SubscriberPtr responsePub = physicsNode->Subscribe("~/response",
|
|
&PhysicsEngineTest::OnPhysicsMsgResponse, this);
|
|
|
|
msgs::Physics_Type type;
|
|
if (_physicsEngine == "ode")
|
|
type = msgs::Physics::ODE;
|
|
else if (_physicsEngine == "bullet")
|
|
type = msgs::Physics::BULLET;
|
|
else if (_physicsEngine == "dart")
|
|
type = msgs::Physics::DART;
|
|
else if (_physicsEngine == "simbody")
|
|
type = msgs::Physics::SIMBODY;
|
|
else
|
|
type = msgs::Physics::ODE;
|
|
physicsPubMsg.set_type(type);
|
|
physicsPubMsg.set_max_step_size(0.01);
|
|
physicsPubMsg.set_real_time_update_rate(500);
|
|
physicsPubMsg.set_real_time_factor(1.2);
|
|
|
|
physicsPub->Publish(physicsPubMsg);
|
|
|
|
msgs::Request *requestMsg = msgs::CreateRequest("physics_info", "");
|
|
requestPub->Publish(*requestMsg);
|
|
|
|
int waitCount = 0, maxWaitCount = 3000;
|
|
while (physicsResponseMsg.ByteSize() == 0 && ++waitCount < maxWaitCount)
|
|
common::Time::MSleep(10);
|
|
|
|
ASSERT_LT(waitCount, maxWaitCount);
|
|
|
|
EXPECT_DOUBLE_EQ(physicsResponseMsg.max_step_size(),
|
|
physicsPubMsg.max_step_size());
|
|
EXPECT_DOUBLE_EQ(physicsResponseMsg.real_time_update_rate(),
|
|
physicsPubMsg.real_time_update_rate());
|
|
EXPECT_DOUBLE_EQ(physicsResponseMsg.real_time_factor(),
|
|
physicsPubMsg.real_time_factor());
|
|
|
|
// Test PhysicsEngine::[GS]etParam()
|
|
{
|
|
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
|
|
boost::any dt = physics->GetParam("max_step_size");
|
|
EXPECT_DOUBLE_EQ(boost::any_cast<double>(dt),
|
|
physicsPubMsg.max_step_size());
|
|
|
|
EXPECT_NO_THROW(physics->GetParam("fake_param_name"));
|
|
EXPECT_NO_THROW(physics->SetParam("fake_param_name", 0));
|
|
|
|
// Try SetParam with wrong type
|
|
EXPECT_NO_THROW(physics->SetParam("iters", std::string("wrong")));
|
|
}
|
|
|
|
{
|
|
// Test SetParam for non-implementation-specific parameters
|
|
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
|
|
try
|
|
{
|
|
boost::any value;
|
|
double maxStepSize = 0.02;
|
|
double realTimeUpdateRate = 0.03;
|
|
double realTimeFactor = 0.04;
|
|
ignition::math::Vector3d gravity(0, 0, 0);
|
|
ignition::math::Vector3d magneticField(0.1, 0.1, 0.1);
|
|
gazebo::math::Vector3 gravity2(0.2, 0.2, 0.2);
|
|
gazebo::math::Vector3 magneticField2(0.3, 0.3, 0.3);
|
|
gzdbg << "Set and Get max_step_size" << std::endl;
|
|
EXPECT_TRUE(physics->SetParam("max_step_size", maxStepSize));
|
|
EXPECT_TRUE(physics->GetParam("max_step_size", value));
|
|
EXPECT_NEAR(boost::any_cast<double>(value), maxStepSize, 1e-6);
|
|
gzdbg << "Set and Get real_time_update_rate" << std::endl;
|
|
EXPECT_TRUE(physics->SetParam("real_time_update_rate",
|
|
realTimeUpdateRate));
|
|
EXPECT_TRUE(physics->GetParam("real_time_update_rate", value));
|
|
EXPECT_NEAR(boost::any_cast<double>(value), realTimeUpdateRate, 1e-6);
|
|
gzdbg << "Set and Get real_time_factor" << std::endl;
|
|
EXPECT_TRUE(physics->SetParam("real_time_factor",
|
|
realTimeFactor));
|
|
EXPECT_TRUE(physics->GetParam("real_time_factor", value));
|
|
EXPECT_NEAR(boost::any_cast<double>(value), realTimeFactor, 1e-6);
|
|
gzdbg << "Set gravity as ignition::math::Vector3d" << std::endl;
|
|
EXPECT_TRUE(physics->SetParam("gravity", gravity));
|
|
EXPECT_TRUE(physics->GetParam("gravity", value));
|
|
EXPECT_EQ(boost::any_cast<math::Vector3>(value), math::Vector3(gravity));
|
|
gzdbg << "Set gravity as gazebo::math::Vector3" << std::endl;
|
|
EXPECT_TRUE(physics->SetParam("gravity", math::Vector3(gravity2)));
|
|
EXPECT_TRUE(physics->GetParam("gravity", value));
|
|
EXPECT_EQ(boost::any_cast<math::Vector3>(value), math::Vector3(gravity2));
|
|
gzdbg << "Set magnetic_field as ignition::math::Vector3d" << std::endl;
|
|
EXPECT_TRUE(physics->SetParam("magnetic_field", magneticField));
|
|
EXPECT_TRUE(physics->GetParam("magnetic_field", value));
|
|
EXPECT_EQ(boost::any_cast<ignition::math::Vector3d>(value),
|
|
magneticField);
|
|
gzdbg << "Set magnetic_field as gazebo::math::Vector3" << std::endl;
|
|
EXPECT_TRUE(physics->SetParam("magnetic_field",
|
|
math::Vector3(magneticField2)));
|
|
EXPECT_TRUE(physics->GetParam("magnetic_field", value));
|
|
EXPECT_EQ(boost::any_cast<ignition::math::Vector3d>(value),
|
|
magneticField2.Ign());
|
|
}
|
|
catch(boost::bad_any_cast &_e)
|
|
{
|
|
std::cout << "Bad any_cast in PhysicsEngine::SetParam test: " << _e.what()
|
|
<< std::endl;
|
|
FAIL();
|
|
}
|
|
}
|
|
|
|
physicsNode->Fini();
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
TEST_P(PhysicsEngineTest, PhysicsEngineParam)
|
|
{
|
|
PhysicsEngineParam(GetParam());
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void PhysicsEngineTest::PhysicsEngineGetParamBool
|
|
(const std::string &_physicsEngine)
|
|
{
|
|
Load("worlds/empty.world", false, _physicsEngine);
|
|
physics::WorldPtr world = physics::get_world("default");
|
|
ASSERT_TRUE(world != NULL);
|
|
|
|
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
|
|
|
|
// Initialize to failure conditions
|
|
boost::any value;
|
|
|
|
// Test shared physics engine parameter(s)
|
|
EXPECT_TRUE(physics->GetParam("gravity", value));
|
|
EXPECT_EQ(boost::any_cast<math::Vector3>(value), math::Vector3(0, 0, -9.8));
|
|
EXPECT_TRUE(physics->GetParam("max_step_size", value));
|
|
EXPECT_NEAR(boost::any_cast<double>(value), 0.001, 1e-6);
|
|
EXPECT_TRUE(physics->GetParam("real_time_factor", value));
|
|
EXPECT_NEAR(boost::any_cast<double>(value), 1.0, 1e-6);
|
|
EXPECT_TRUE(physics->GetParam("real_time_update_rate", value));
|
|
EXPECT_NEAR(boost::any_cast<double>(value), 1000.0, 1e-6);
|
|
EXPECT_TRUE(physics->GetParam("type", value));
|
|
EXPECT_EQ(boost::any_cast<std::string>(value), _physicsEngine);
|
|
|
|
if (_physicsEngine == "ode" || _physicsEngine == "bullet")
|
|
{
|
|
EXPECT_TRUE(physics->GetParam("iters", value));
|
|
EXPECT_EQ(boost::any_cast<int>(value), 50);
|
|
}
|
|
else if (_physicsEngine == "dart")
|
|
{
|
|
gzwarn << "DARTPhysics::GetParam not yet implemented." << std::endl;
|
|
return;
|
|
}
|
|
else if (_physicsEngine == "simbody")
|
|
{
|
|
EXPECT_TRUE(physics->GetParam("accuracy", value));
|
|
EXPECT_NEAR(boost::any_cast<double>(value), 1e-3, 1e-6);
|
|
}
|
|
|
|
EXPECT_FALSE(physics->GetParam("param_does_not_exist", value));
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
TEST_P(PhysicsEngineTest, PhysicsEngineGetParamBool)
|
|
{
|
|
PhysicsEngineGetParamBool(GetParam());
|
|
}
|
|
|
|
INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsEngineTest,
|
|
PHYSICS_ENGINE_VALUES);
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
::testing::InitGoogleTest(&argc, argv);
|
|
return RUN_ALL_TESTS();
|
|
}
|