375 lines
13 KiB
C++
375 lines
13 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 <string.h>
|
||
|
|
#include "gazebo/math/Helpers.hh"
|
||
|
|
#include "gazebo/transport/TransportTypes.hh"
|
||
|
|
#include "gazebo/transport/Node.hh"
|
||
|
|
|
||
|
|
#include "gazebo/rendering/RenderEngine.hh"
|
||
|
|
#include "gazebo/rendering/Camera.hh"
|
||
|
|
#include "gazebo/sensors/SensorsIface.hh"
|
||
|
|
#include "gazebo/sensors/CameraSensor.hh"
|
||
|
|
#include "gazebo/test/ServerFixture.hh"
|
||
|
|
#include "images_cmp.h"
|
||
|
|
#include "gazebo/test/helper_physics_generator.hh"
|
||
|
|
|
||
|
|
using namespace gazebo;
|
||
|
|
class FactoryTest : public ServerFixture,
|
||
|
|
public testing::WithParamInterface<const char*>
|
||
|
|
{
|
||
|
|
public: void BoxSdf(const std::string &_physicsEngine);
|
||
|
|
public: void Box(const std::string &_physicsEngine);
|
||
|
|
public: void Sphere(const std::string &_physicsEngine);
|
||
|
|
public: void Cylinder(const std::string &_physicsEngine);
|
||
|
|
public: void Clone(const std::string &_physicsEngine);
|
||
|
|
};
|
||
|
|
|
||
|
|
|
||
|
|
class LightFactoryTest : public ServerFixture
|
||
|
|
{
|
||
|
|
/// \brief Light factory publisher.
|
||
|
|
protected: transport::PublisherPtr lightFactoryPub;
|
||
|
|
};
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////
|
||
|
|
// Verify that sdf is retained by entities spawned
|
||
|
|
// via factory messages. A change between 1.6, 1.7
|
||
|
|
// caused entities to lose their sdf data
|
||
|
|
// (see issue #651)
|
||
|
|
void FactoryTest::BoxSdf(const std::string &_physicsEngine)
|
||
|
|
{
|
||
|
|
ignition::math::Pose3d setPose;
|
||
|
|
Load("worlds/empty.world", true, _physicsEngine);
|
||
|
|
physics::WorldPtr world = physics::get_world("default");
|
||
|
|
ASSERT_TRUE(world != NULL);
|
||
|
|
|
||
|
|
unsigned int entityCount = 6;
|
||
|
|
|
||
|
|
for (unsigned int i = 0; i < entityCount; ++i)
|
||
|
|
{
|
||
|
|
std::ostringstream name;
|
||
|
|
name << "test_box_" << i;
|
||
|
|
setPose.Set(ignition::math::Vector3d(0, 0, i+0.5),
|
||
|
|
ignition::math::Quaterniond(0, 0, 0));
|
||
|
|
SpawnBox(name.str(), ignition::math::Vector3d(1, 1, 1), setPose.Pos(),
|
||
|
|
setPose.Rot().Euler());
|
||
|
|
}
|
||
|
|
|
||
|
|
// This loop must be separate from the previous loop to cause
|
||
|
|
// the failure.
|
||
|
|
for (unsigned int i = 0; i < entityCount; ++i)
|
||
|
|
{
|
||
|
|
std::ostringstream name;
|
||
|
|
name << "test_box_" << i;
|
||
|
|
|
||
|
|
physics::ModelPtr model = world->GetModel(name.str());
|
||
|
|
ASSERT_TRUE(model != NULL);
|
||
|
|
msgs::Model msg;
|
||
|
|
model->FillMsg(msg);
|
||
|
|
EXPECT_TRUE(msg.has_pose());
|
||
|
|
// gzerr << msg.DebugString() << '\n';
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_P(FactoryTest, BoxSdf)
|
||
|
|
{
|
||
|
|
BoxSdf(GetParam());
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void FactoryTest::Box(const std::string &_physicsEngine)
|
||
|
|
{
|
||
|
|
ignition::math::Pose3d setPose, testPose;
|
||
|
|
Load("worlds/empty.world", true, _physicsEngine);
|
||
|
|
|
||
|
|
for (unsigned int i = 0; i < 100; i++)
|
||
|
|
{
|
||
|
|
std::ostringstream name;
|
||
|
|
name << "test_box_" << i;
|
||
|
|
setPose.Set(ignition::math::Vector3d(0, 0, i+0.5),
|
||
|
|
ignition::math::Quaterniond(0, 0, 0));
|
||
|
|
SpawnBox(name.str(), math::Vector3(1, 1, 1), setPose.Pos(),
|
||
|
|
setPose.Rot().Euler());
|
||
|
|
testPose = GetEntityPose(name.str()).Ign();
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().X(), setPose.Pos().X(), 0.1));
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().Y(), setPose.Pos().Y(), 0.1));
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().Z(), setPose.Pos().Z(), 0.1));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_P(FactoryTest, Box)
|
||
|
|
{
|
||
|
|
Box(GetParam());
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void FactoryTest::Sphere(const std::string &_physicsEngine)
|
||
|
|
{
|
||
|
|
ignition::math::Pose3d setPose, testPose;
|
||
|
|
Load("worlds/empty.world", true, _physicsEngine);
|
||
|
|
|
||
|
|
for (unsigned int i = 0; i < 100; i++)
|
||
|
|
{
|
||
|
|
std::ostringstream name;
|
||
|
|
name << "test_sphere_" << i;
|
||
|
|
setPose.Set(ignition::math::Vector3d(0, 0, i+0.5),
|
||
|
|
ignition::math::Quaterniond(0, 0, 0));
|
||
|
|
SpawnSphere(name.str(), setPose.Pos(), setPose.Rot().Euler());
|
||
|
|
testPose = GetEntityPose(name.str()).Ign();
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().X(), setPose.Pos().X(), 0.1));
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().Y(), setPose.Pos().Y(), 0.1));
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().Z(), setPose.Pos().Z(), 0.1));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_P(FactoryTest, Sphere)
|
||
|
|
{
|
||
|
|
Sphere(GetParam());
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void FactoryTest::Cylinder(const std::string &_physicsEngine)
|
||
|
|
{
|
||
|
|
ignition::math::Pose3d setPose, testPose;
|
||
|
|
Load("worlds/empty.world", true, _physicsEngine);
|
||
|
|
|
||
|
|
for (unsigned int i = 0; i < 100; i++)
|
||
|
|
{
|
||
|
|
std::ostringstream name;
|
||
|
|
name << "test_cylinder_" << i;
|
||
|
|
setPose.Set(
|
||
|
|
ignition::math::Vector3d(0, 0, i+0.5),
|
||
|
|
ignition::math::Quaterniond(0, 0, 0));
|
||
|
|
SpawnCylinder(name.str(), setPose.Pos(), setPose.Rot().Euler());
|
||
|
|
testPose = GetEntityPose(name.str()).Ign();
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().X(), setPose.Pos().X(), 0.1));
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().Y(), setPose.Pos().Y(), 0.1));
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().Z(), setPose.Pos().Z(), 0.1));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_P(FactoryTest, Cylinder)
|
||
|
|
{
|
||
|
|
Cylinder(GetParam());
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void FactoryTest::Clone(const std::string &_physicsEngine)
|
||
|
|
{
|
||
|
|
ignition::math::Pose3d testPose;
|
||
|
|
Load("worlds/pr2.world", true, _physicsEngine);
|
||
|
|
|
||
|
|
// clone the pr2
|
||
|
|
std::string name = "pr2";
|
||
|
|
msgs::Factory msg;
|
||
|
|
ignition::math::Pose3d clonePose;
|
||
|
|
clonePose.Set(ignition::math::Vector3d(2, 3, 0.5),
|
||
|
|
ignition::math::Quaterniond(0, 0, 0));
|
||
|
|
msgs::Set(msg.mutable_pose(), clonePose);
|
||
|
|
msg.set_clone_model_name(name);
|
||
|
|
this->factoryPub->Publish(msg);
|
||
|
|
|
||
|
|
// Wait for the pr2 clone to spawn
|
||
|
|
std::string cloneName = name + "_clone";
|
||
|
|
this->WaitUntilEntitySpawn(cloneName, 100, 100);
|
||
|
|
|
||
|
|
EXPECT_TRUE(this->HasEntity(cloneName));
|
||
|
|
testPose = GetEntityPose(cloneName).Ign();
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().X(), clonePose.Pos().X(), 0.1));
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().Y(), clonePose.Pos().Y(), 0.1));
|
||
|
|
EXPECT_TRUE(math::equal(testPose.Pos().Z(), clonePose.Pos().Z(), 0.1));
|
||
|
|
|
||
|
|
// Verify properties of the pr2 clone with the original model.
|
||
|
|
physics::WorldPtr world = physics::get_world("default");
|
||
|
|
ASSERT_TRUE(world != NULL);
|
||
|
|
|
||
|
|
// Check model
|
||
|
|
physics::ModelPtr model = world->GetModel(name);
|
||
|
|
ASSERT_TRUE(model != NULL);
|
||
|
|
physics::ModelPtr modelClone = world->GetModel(cloneName);
|
||
|
|
ASSERT_TRUE(modelClone != NULL);
|
||
|
|
EXPECT_EQ(model->GetJointCount(), modelClone->GetJointCount());
|
||
|
|
EXPECT_EQ(model->GetLinks().size(), modelClone->GetLinks().size());
|
||
|
|
EXPECT_EQ(model->GetSensorCount(), modelClone->GetSensorCount());
|
||
|
|
EXPECT_EQ(model->GetPluginCount(), modelClone->GetPluginCount());
|
||
|
|
EXPECT_EQ(model->GetAutoDisable(), modelClone->GetAutoDisable());
|
||
|
|
|
||
|
|
// Check links
|
||
|
|
physics::Link_V links = model->GetLinks();
|
||
|
|
physics::Link_V linkClones = modelClone->GetLinks();
|
||
|
|
for (unsigned int i = 0; i < links.size(); ++i)
|
||
|
|
{
|
||
|
|
physics::LinkPtr link = links[i];
|
||
|
|
physics::LinkPtr linkClone = linkClones[i];
|
||
|
|
|
||
|
|
EXPECT_EQ(link->GetSensorCount(), linkClone->GetSensorCount());
|
||
|
|
EXPECT_EQ(link->GetKinematic(), linkClone->GetKinematic());
|
||
|
|
|
||
|
|
// Check collisions
|
||
|
|
physics::Collision_V collisions = link->GetCollisions();
|
||
|
|
physics::Collision_V collisionClones = linkClone->GetCollisions();
|
||
|
|
EXPECT_EQ(collisions.size(), collisionClones.size());
|
||
|
|
for (unsigned int j = 0; j < collisions.size(); ++j)
|
||
|
|
{
|
||
|
|
physics::CollisionPtr collision = collisions[j];
|
||
|
|
physics::CollisionPtr collisionClone = collisionClones[j];
|
||
|
|
EXPECT_EQ(collision->GetShapeType(), collisionClone->GetShapeType());
|
||
|
|
EXPECT_EQ(collision->GetMaxContacts(), collisionClone->GetMaxContacts());
|
||
|
|
|
||
|
|
// Check surface
|
||
|
|
physics::SurfaceParamsPtr surface = collision->GetSurface();
|
||
|
|
physics::SurfaceParamsPtr cloneSurface = collisionClone->GetSurface();
|
||
|
|
EXPECT_EQ(surface->collideWithoutContact,
|
||
|
|
cloneSurface->collideWithoutContact);
|
||
|
|
EXPECT_EQ(surface->collideWithoutContactBitmask,
|
||
|
|
cloneSurface->collideWithoutContactBitmask);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Check inertial
|
||
|
|
physics::InertialPtr inertial = link->GetInertial();
|
||
|
|
physics::InertialPtr inertialClone = linkClone->GetInertial();
|
||
|
|
EXPECT_EQ(inertial->GetMass(), inertialClone->GetMass());
|
||
|
|
EXPECT_EQ(inertial->GetCoG(), inertialClone->GetCoG());
|
||
|
|
EXPECT_EQ(inertial->GetPrincipalMoments(),
|
||
|
|
inertialClone->GetPrincipalMoments());
|
||
|
|
EXPECT_EQ(inertial->GetProductsofInertia(),
|
||
|
|
inertialClone->GetProductsofInertia());
|
||
|
|
}
|
||
|
|
|
||
|
|
// Check joints
|
||
|
|
physics::Joint_V joints = model->GetJoints();
|
||
|
|
physics::Joint_V jointClones = modelClone->GetJoints();
|
||
|
|
for (unsigned int i = 0; i < joints.size(); ++i)
|
||
|
|
{
|
||
|
|
physics::JointPtr joint = joints[i];
|
||
|
|
physics::JointPtr jointClone = jointClones[i];
|
||
|
|
EXPECT_EQ(joint->GetAngleCount(), jointClone->GetAngleCount());
|
||
|
|
for (unsigned j = 0; j < joint->GetAngleCount(); ++j)
|
||
|
|
{
|
||
|
|
EXPECT_EQ(joint->GetUpperLimit(j), jointClone->GetUpperLimit(j));
|
||
|
|
EXPECT_EQ(joint->GetLowerLimit(j), jointClone->GetLowerLimit(j));
|
||
|
|
EXPECT_EQ(joint->GetEffortLimit(j), jointClone->GetEffortLimit(j));
|
||
|
|
EXPECT_EQ(joint->GetVelocityLimit(j), jointClone->GetVelocityLimit(j));
|
||
|
|
EXPECT_EQ(joint->GetStopStiffness(j), jointClone->GetStopStiffness(j));
|
||
|
|
EXPECT_EQ(joint->GetStopDissipation(j),
|
||
|
|
jointClone->GetStopDissipation(j));
|
||
|
|
EXPECT_EQ(joint->GetLocalAxis(j), jointClone->GetLocalAxis(j));
|
||
|
|
EXPECT_EQ(joint->GetDamping(j), jointClone->GetDamping(j));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_P(FactoryTest, Clone)
|
||
|
|
{
|
||
|
|
Clone(GetParam());
|
||
|
|
}
|
||
|
|
|
||
|
|
// Disabling this test for now. Different machines return different
|
||
|
|
// camera images. Need a better way to evaluate rendered content.
|
||
|
|
// TEST_F(FactoryTest, Camera)
|
||
|
|
// {
|
||
|
|
/*
|
||
|
|
if (rendering::RenderEngine::Instance()->GetRenderPathType() ==
|
||
|
|
rendering::RenderEngine::NONE)
|
||
|
|
return;
|
||
|
|
|
||
|
|
math::Pose setPose, testPose;
|
||
|
|
Load("worlds/empty.world");
|
||
|
|
setPose.Set(math::Vector3(-5, 0, 5), math::Quaternion(0, GZ_DTOR(15), 0));
|
||
|
|
SpawnCamera("camera_model", "camera_sensor2", setPose.pos,
|
||
|
|
setPose.rot.GetAsEuler());
|
||
|
|
|
||
|
|
unsigned char *img = NULL;
|
||
|
|
unsigned int width;
|
||
|
|
unsigned int height;
|
||
|
|
GetFrame("camera_sensor2", &img, width, height);
|
||
|
|
ASSERT_EQ(width, static_cast<unsigned int>(320));
|
||
|
|
ASSERT_EQ(height, static_cast<unsigned int>(240));
|
||
|
|
|
||
|
|
unsigned int diffMax = 0;
|
||
|
|
unsigned int diffSum = 0;
|
||
|
|
double diffAvg = 0;
|
||
|
|
ImageCompare(&img, &empty_world_camera1,
|
||
|
|
width, height, 3, diffMax, diffSum, diffAvg);
|
||
|
|
// PrintImage("empty_world_camera1", &img, width, height, 3);
|
||
|
|
ASSERT_LT(diffSum, static_cast<unsigned int>(100));
|
||
|
|
ASSERT_EQ(static_cast<unsigned int>(0), diffMax);
|
||
|
|
ASSERT_EQ(0.0, diffAvg);
|
||
|
|
*/
|
||
|
|
// }
|
||
|
|
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_F(LightFactoryTest, SpawnLight)
|
||
|
|
{
|
||
|
|
Load("worlds/empty.world");
|
||
|
|
|
||
|
|
physics::WorldPtr world = physics::get_world("default");
|
||
|
|
ASSERT_TRUE(world != nullptr);
|
||
|
|
|
||
|
|
// create lights using ~/factory/light topic
|
||
|
|
this->lightFactoryPub = this->node->Advertise<msgs::Light>("~/factory/light");
|
||
|
|
std::string light1Name = "new1_light";
|
||
|
|
std::string light2Name = "new2_light";
|
||
|
|
|
||
|
|
msgs::Light msg;
|
||
|
|
msg.set_name(light1Name);
|
||
|
|
ignition::math::Pose3d light1Pose = ignition::math::Pose3d(0, 2, 1, 0, 0, 0);
|
||
|
|
msgs::Set(msg.mutable_pose(), light1Pose);
|
||
|
|
this->lightFactoryPub->Publish(msg);
|
||
|
|
|
||
|
|
msg.set_name(light2Name);
|
||
|
|
ignition::math::Pose3d light2Pose = ignition::math::Pose3d(1, 0, 5, 0, 2, 0);
|
||
|
|
msgs::Set(msg.mutable_pose(), light2Pose);
|
||
|
|
this->lightFactoryPub->Publish(msg);
|
||
|
|
|
||
|
|
// wait for light to spawn
|
||
|
|
int sleep = 0;
|
||
|
|
int maxSleep = 50;
|
||
|
|
while ((!world->Light(light1Name) || !world->Light(light2Name)) &&
|
||
|
|
sleep++ < maxSleep)
|
||
|
|
{
|
||
|
|
common::Time::MSleep(100);
|
||
|
|
}
|
||
|
|
|
||
|
|
// verify lights in world
|
||
|
|
physics::LightPtr light1 = world->Light(light1Name);
|
||
|
|
physics::LightPtr light2 = world->Light(light2Name);
|
||
|
|
ASSERT_NE(nullptr, light1);
|
||
|
|
ASSERT_NE(nullptr, light2);
|
||
|
|
|
||
|
|
EXPECT_EQ(light1Name, light1->GetScopedName());
|
||
|
|
EXPECT_EQ(light1Pose, light1->GetWorldPose().Ign());
|
||
|
|
|
||
|
|
EXPECT_EQ(light2Name, light2->GetScopedName());
|
||
|
|
EXPECT_EQ(light2Pose, light2->GetWorldPose().Ign());
|
||
|
|
}
|
||
|
|
|
||
|
|
INSTANTIATE_TEST_CASE_P(PhysicsEngines, FactoryTest, PHYSICS_ENGINE_VALUES);
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
int main(int argc, char **argv)
|
||
|
|
{
|
||
|
|
::testing::InitGoogleTest(&argc, argv);
|
||
|
|
return RUN_ALL_TESTS();
|
||
|
|
}
|