261 lines
8.9 KiB
C++
261 lines
8.9 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2015 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>
|
||
|
|
|
||
|
|
#include <ignition/math/Vector3.hh>
|
||
|
|
|
||
|
|
#include "gazebo/msgs/msgs.hh"
|
||
|
|
#include "gazebo/physics/physics.hh"
|
||
|
|
#include "gazebo/transport/transport.hh"
|
||
|
|
#include "gazebo/test/ServerFixture.hh"
|
||
|
|
#include "gazebo/test/helper_physics_generator.hh"
|
||
|
|
|
||
|
|
using namespace gazebo;
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
class SavingLoadingTest : public ServerFixture,
|
||
|
|
public testing::WithParamInterface<const char*>
|
||
|
|
{
|
||
|
|
/// \brief Test scaling a model and then saving it. The scale should show up
|
||
|
|
/// in the state SDF instead of the initial model SDF.
|
||
|
|
/// \param[in] _physicsEngine Physics Engine type.
|
||
|
|
public: void SaveScaledModel(const std::string &_physicsEngine);
|
||
|
|
|
||
|
|
/// \brief Test loading a world with models scaled in the world state.
|
||
|
|
/// \param[in] _physicsEngine Physics Engine type.
|
||
|
|
public: void LoadScaledModels(const std::string &_physicsEngine);
|
||
|
|
};
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
void SavingLoadingTest::SaveScaledModel(const std::string &_physicsEngine)
|
||
|
|
{
|
||
|
|
// load a world with simple shapes
|
||
|
|
this->Load("worlds/shapes.world", true, _physicsEngine);
|
||
|
|
auto world = physics::get_world("default");
|
||
|
|
ASSERT_TRUE(world != NULL);
|
||
|
|
|
||
|
|
auto physics = world->GetPhysicsEngine();
|
||
|
|
ASSERT_TRUE(physics != NULL);
|
||
|
|
|
||
|
|
// Get box and check scale
|
||
|
|
auto box = world->GetModel("box");
|
||
|
|
ASSERT_TRUE(box != NULL);
|
||
|
|
EXPECT_EQ(box->Scale(), ignition::math::Vector3d::One);
|
||
|
|
|
||
|
|
// Create transport
|
||
|
|
auto node = transport::NodePtr(new transport::Node());
|
||
|
|
node->Init();
|
||
|
|
auto modelPub = node->Advertise<msgs::Model>("~/model/modify");
|
||
|
|
|
||
|
|
// scale box
|
||
|
|
ignition::math::Vector3d newScale(0.1, 2, 3);
|
||
|
|
msgs::Model msg;
|
||
|
|
msg.set_name("box");
|
||
|
|
|
||
|
|
msgs::Set(msg.mutable_scale(), newScale);
|
||
|
|
modelPub->Publish(msg);
|
||
|
|
|
||
|
|
// Check box has been scaled
|
||
|
|
int sleep = 0;
|
||
|
|
int maxSleep = 10;
|
||
|
|
while (box->Scale() != newScale && sleep < maxSleep)
|
||
|
|
{
|
||
|
|
gazebo::common::Time::MSleep(100);
|
||
|
|
sleep++;
|
||
|
|
}
|
||
|
|
EXPECT_EQ(box->Scale(), newScale);
|
||
|
|
|
||
|
|
// request SDF to save world
|
||
|
|
auto response = transport::request("default", "world_sdf_save");
|
||
|
|
EXPECT_NE(response->response(), "error");
|
||
|
|
|
||
|
|
std::string str = response->serialized_data();
|
||
|
|
// Remove strange characters from the front
|
||
|
|
str = str.substr(3);
|
||
|
|
|
||
|
|
sdf::SDF sdf_parsed;
|
||
|
|
sdf_parsed.SetFromString(str);
|
||
|
|
ASSERT_TRUE(sdf_parsed.Root()->HasElement("world"));
|
||
|
|
|
||
|
|
auto worldElem = sdf_parsed.Root()->GetElement("world");
|
||
|
|
|
||
|
|
// Check all model sizes (scales)
|
||
|
|
EXPECT_TRUE(worldElem->HasElement("model"));
|
||
|
|
auto modelElem = worldElem->GetElement("model");
|
||
|
|
while (modelElem)
|
||
|
|
{
|
||
|
|
EXPECT_TRUE(modelElem->HasElement("link"));
|
||
|
|
EXPECT_TRUE(modelElem->GetElement("link")->HasElement("visual"));
|
||
|
|
EXPECT_TRUE(modelElem->GetElement("link")->HasElement("collision"));
|
||
|
|
EXPECT_TRUE(modelElem->GetElement("link")->GetElement("visual")->
|
||
|
|
HasElement("geometry"));
|
||
|
|
EXPECT_TRUE(modelElem->GetElement("link")->GetElement("collision")->
|
||
|
|
HasElement("geometry"));
|
||
|
|
|
||
|
|
auto visualGeom = modelElem->GetElement("link")->GetElement("visual")->
|
||
|
|
GetElement("geometry");
|
||
|
|
auto collisionGeom = modelElem->GetElement("link")->
|
||
|
|
GetElement("collision")->GetElement("geometry");
|
||
|
|
|
||
|
|
EXPECT_TRUE(modelElem->HasAttribute("name"));
|
||
|
|
auto name = modelElem->GetAttribute("name")->GetAsString();
|
||
|
|
|
||
|
|
// Check that all sizes are still one
|
||
|
|
if (name == "box")
|
||
|
|
{
|
||
|
|
gzdbg << "Checking box model geometry size." << std::endl;
|
||
|
|
EXPECT_TRUE(visualGeom->HasElement("box"));
|
||
|
|
EXPECT_TRUE(collisionGeom->HasElement("box"));
|
||
|
|
EXPECT_TRUE(visualGeom->GetElement("box")->HasElement("size"));
|
||
|
|
EXPECT_TRUE(collisionGeom->GetElement("box")->HasElement("size"));
|
||
|
|
|
||
|
|
auto visualSize = visualGeom->GetElement("box")->
|
||
|
|
Get<ignition::math::Vector3d>("size");
|
||
|
|
auto collisionSize = collisionGeom->GetElement("box")->
|
||
|
|
Get<ignition::math::Vector3d>("size");
|
||
|
|
|
||
|
|
EXPECT_EQ(visualSize, ignition::math::Vector3d::One);
|
||
|
|
EXPECT_EQ(collisionSize, ignition::math::Vector3d::One);
|
||
|
|
}
|
||
|
|
else if (name == "sphere")
|
||
|
|
{
|
||
|
|
gzdbg << "Checking sphere model geometry size." << std::endl;
|
||
|
|
EXPECT_TRUE(visualGeom->HasElement("sphere"));
|
||
|
|
EXPECT_TRUE(collisionGeom->HasElement("sphere"));
|
||
|
|
EXPECT_TRUE(visualGeom->GetElement("sphere")->HasElement("radius"));
|
||
|
|
EXPECT_TRUE(collisionGeom->GetElement("sphere")->HasElement("radius"));
|
||
|
|
|
||
|
|
auto visualSize = visualGeom->GetElement("sphere")->Get<double>("radius");
|
||
|
|
auto collisionSize = collisionGeom->GetElement("sphere")->
|
||
|
|
Get<double>("radius");
|
||
|
|
|
||
|
|
EXPECT_EQ(visualSize, 0.5);
|
||
|
|
EXPECT_EQ(collisionSize, 0.5);
|
||
|
|
}
|
||
|
|
else if (name == "cylinder")
|
||
|
|
{
|
||
|
|
gzdbg << "Checking cylinder model geometry size." << std::endl;
|
||
|
|
EXPECT_TRUE(visualGeom->HasElement("cylinder"));
|
||
|
|
EXPECT_TRUE(collisionGeom->HasElement("cylinder"));
|
||
|
|
EXPECT_TRUE(visualGeom->GetElement("cylinder")->HasElement("radius"));
|
||
|
|
EXPECT_TRUE(collisionGeom->GetElement("cylinder")->HasElement("radius"));
|
||
|
|
EXPECT_TRUE(visualGeom->GetElement("cylinder")->HasElement("length"));
|
||
|
|
EXPECT_TRUE(collisionGeom->GetElement("cylinder")->HasElement("length"));
|
||
|
|
|
||
|
|
auto visualRadius = visualGeom->GetElement("cylinder")->
|
||
|
|
Get<double>("radius");
|
||
|
|
auto collisionRadius = collisionGeom->GetElement("cylinder")->
|
||
|
|
Get<double>("radius");
|
||
|
|
auto visualLength = visualGeom->GetElement("cylinder")->
|
||
|
|
Get<double>("length");
|
||
|
|
auto collisionLength = collisionGeom->GetElement("cylinder")->
|
||
|
|
Get<double>("length");
|
||
|
|
|
||
|
|
EXPECT_EQ(visualRadius, 0.5);
|
||
|
|
EXPECT_EQ(collisionRadius, 0.5);
|
||
|
|
EXPECT_EQ(visualLength, 1.0);
|
||
|
|
EXPECT_EQ(collisionLength, 1.0);
|
||
|
|
}
|
||
|
|
|
||
|
|
modelElem = modelElem->GetNextElement("model");
|
||
|
|
}
|
||
|
|
|
||
|
|
// Check scale in all model states
|
||
|
|
EXPECT_TRUE(worldElem->HasElement("state"));
|
||
|
|
EXPECT_TRUE(worldElem->GetElement("state")->HasElement("model"));
|
||
|
|
auto modelStateElem = worldElem->GetElement("state")->GetElement("model");
|
||
|
|
while (modelStateElem)
|
||
|
|
{
|
||
|
|
EXPECT_TRUE(modelStateElem->HasAttribute("name"));
|
||
|
|
auto name = modelStateElem->GetAttribute("name")->GetAsString();
|
||
|
|
|
||
|
|
EXPECT_TRUE(modelStateElem->HasElement("scale"));
|
||
|
|
auto scale = modelStateElem->Get<ignition::math::Vector3d>("scale");
|
||
|
|
|
||
|
|
// Check that only box state has new scale
|
||
|
|
if (name == "box")
|
||
|
|
{
|
||
|
|
gzdbg << "Checking box model state scale." << std::endl;
|
||
|
|
EXPECT_EQ(scale, newScale);
|
||
|
|
}
|
||
|
|
else if (name == "sphere")
|
||
|
|
{
|
||
|
|
gzdbg << "Checking sphere model state scale." << std::endl;
|
||
|
|
EXPECT_EQ(scale, ignition::math::Vector3d::One);
|
||
|
|
}
|
||
|
|
else if (name == "cylinder")
|
||
|
|
{
|
||
|
|
gzdbg << "Checking cylinder model state scale." << std::endl;
|
||
|
|
EXPECT_EQ(scale, ignition::math::Vector3d::One);
|
||
|
|
}
|
||
|
|
|
||
|
|
modelStateElem = modelStateElem->GetNextElement("model");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
void SavingLoadingTest::LoadScaledModels(const std::string &_physicsEngine)
|
||
|
|
{
|
||
|
|
// load a world which has models scaled in the state
|
||
|
|
this->Load("test/worlds/scaled_shapes.world", true, _physicsEngine);
|
||
|
|
auto world = physics::get_world("default");
|
||
|
|
ASSERT_TRUE(world != NULL);
|
||
|
|
|
||
|
|
auto physics = world->GetPhysicsEngine();
|
||
|
|
ASSERT_TRUE(physics != NULL);
|
||
|
|
|
||
|
|
// Get box and check scale
|
||
|
|
auto box = world->GetModel("box");
|
||
|
|
ASSERT_TRUE(box != NULL);
|
||
|
|
EXPECT_EQ(box->Scale(), ignition::math::Vector3d(3.34637, 2.87999, 0.123722));
|
||
|
|
|
||
|
|
// Get sphere and check scale
|
||
|
|
auto sphere = world->GetModel("sphere");
|
||
|
|
ASSERT_TRUE(sphere != NULL);
|
||
|
|
EXPECT_EQ(sphere->Scale(),
|
||
|
|
ignition::math::Vector3d(0.402674, 0.402674, 0.402674));
|
||
|
|
|
||
|
|
// Get cylinder and check scale
|
||
|
|
auto cylinder = world->GetModel("cylinder");
|
||
|
|
ASSERT_TRUE(cylinder != NULL);
|
||
|
|
EXPECT_EQ(cylinder->Scale(),
|
||
|
|
ignition::math::Vector3d(2.2969, 2.2969, 2.09564));
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_P(SavingLoadingTest, SaveScaledModel)
|
||
|
|
{
|
||
|
|
this->SaveScaledModel(GetParam());
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_P(SavingLoadingTest, LoadScaledModels)
|
||
|
|
{
|
||
|
|
this->LoadScaledModels(GetParam());
|
||
|
|
}
|
||
|
|
|
||
|
|
INSTANTIATE_TEST_CASE_P(PhysicsEngines, SavingLoadingTest,
|
||
|
|
PHYSICS_ENGINE_VALUES);
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
int main(int argc, char **argv)
|
||
|
|
{
|
||
|
|
::testing::InitGoogleTest(&argc, argv);
|
||
|
|
return RUN_ALL_TESTS();
|
||
|
|
}
|