361 lines
12 KiB
C++
361 lines
12 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 "gazebo/test/ServerFixture.hh"
|
|
#include "test/util.hh"
|
|
#include "gazebo/physics/PhysicsTypes.hh"
|
|
#include "gazebo/physics/World.hh"
|
|
#include "gazebo/physics/WorldState.hh"
|
|
|
|
using namespace gazebo;
|
|
|
|
class WorldStateTest : public ServerFixture { };
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(WorldStateTest, SDFConstructor)
|
|
{
|
|
// Create the state sdf
|
|
std::ostringstream sdfStr;
|
|
sdfStr << "<sdf version ='" << SDF_VERSION << "'>"
|
|
<< "<world name='default'>"
|
|
<< "<state world_name='world_state_name'>"
|
|
<< "<model name='ground_plane'>"
|
|
<< " <pose>1 1 1 0 0 0</pose>"
|
|
<< " <link name='link'>"
|
|
<< " <pose>0.1 0.2 0.3 0.4 0.5 0.6</pose>"
|
|
<< " </link>"
|
|
<< "</model>"
|
|
<< "<model name='model_1'>"
|
|
<< " <pose>2 1 1 0 0 0</pose>"
|
|
<< " <link name='link_1'>"
|
|
<< " </link>"
|
|
<< "</model>"
|
|
<< "<light name='sun'>"
|
|
<< " <pose>10 20 30 0 0 0</pose>"
|
|
<< "</light>"
|
|
<< "<light name='light_1'>"
|
|
<< " <pose>1 2 3 0 0 0</pose>"
|
|
<< "</light>"
|
|
<< "</state>"
|
|
<< "</world>"
|
|
<< "</sdf>";
|
|
|
|
sdf::SDFPtr worldSDF(new sdf::SDF);
|
|
|
|
worldSDF->SetFromString(sdfStr.str());
|
|
EXPECT_TRUE(worldSDF->Root()->HasElement("world"));
|
|
|
|
auto worldElem = worldSDF->Root()->GetElement("world");
|
|
EXPECT_TRUE(worldElem->HasElement("state"));
|
|
|
|
// Create the world state
|
|
physics::WorldState worldState(worldElem->GetElement("state"));
|
|
|
|
// Check world state against values from the sdf string
|
|
EXPECT_EQ(worldState.GetModelStateCount(), 2u);
|
|
EXPECT_EQ(worldState.LightStateCount(), 2u);
|
|
|
|
auto modelStates = worldState.GetModelStates();
|
|
EXPECT_EQ(modelStates.size(), 2u);
|
|
EXPECT_EQ(modelStates["ground_plane"].GetPose(),
|
|
ignition::math::Pose3d(1, 1, 1, 0, 0, 0));
|
|
EXPECT_EQ(modelStates["model_1"].GetPose(),
|
|
ignition::math::Pose3d(2, 1, 1, 0, 0, 0));
|
|
EXPECT_EQ(modelStates["fake_model"].GetPose(),
|
|
ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
|
|
|
|
auto lightStates = worldState.LightStates();
|
|
EXPECT_EQ(lightStates.size(), 2u);
|
|
EXPECT_EQ(lightStates["sun"].Pose(),
|
|
ignition::math::Pose3d(10, 20, 30, 0, 0, 0));
|
|
EXPECT_EQ(lightStates["light_1"].Pose(),
|
|
ignition::math::Pose3d(1, 2, 3, 0, 0, 0));
|
|
EXPECT_EQ(lightStates["fake_light"].Pose(),
|
|
ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(WorldStateTest, WorldConstructor)
|
|
{
|
|
// Load a world
|
|
this->Load("worlds/empty.world", true);
|
|
physics::WorldPtr world = physics::get_world("default");
|
|
|
|
// Create the world state
|
|
physics::WorldState worldState(world);
|
|
|
|
// Check world state
|
|
EXPECT_EQ(worldState.GetModelStateCount(), 1u);
|
|
EXPECT_EQ(worldState.LightStateCount(), 1u);
|
|
|
|
auto modelStates = worldState.GetModelStates();
|
|
EXPECT_EQ(modelStates.size(), 1u);
|
|
EXPECT_EQ(modelStates["ground_plane"].GetPose(),
|
|
ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
|
|
|
|
auto lightStates = worldState.LightStates();
|
|
EXPECT_EQ(lightStates.size(), 1u);
|
|
EXPECT_EQ(lightStates["sun"].Pose(),
|
|
ignition::math::Pose3d(0, 0, 10, 0, 0, 0));
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(WorldStateTest, FillSDF)
|
|
{
|
|
// Load a world
|
|
this->Load("worlds/empty.world", true);
|
|
physics::WorldPtr world = physics::get_world("default");
|
|
|
|
// Create the world state
|
|
physics::WorldState worldState(world);
|
|
|
|
// Fill SDF
|
|
sdf::ElementPtr filledSDF(new sdf::Element);
|
|
sdf::initFile("state.sdf", filledSDF);
|
|
worldState.FillSDF(filledSDF);
|
|
|
|
EXPECT_TRUE(filledSDF->HasAttribute("world_name"));
|
|
EXPECT_EQ(filledSDF->Get<std::string>("world_name"),
|
|
"default");
|
|
|
|
EXPECT_TRUE(filledSDF->HasElement("model"));
|
|
EXPECT_EQ(filledSDF->GetElement("model")->Get<std::string>("name"),
|
|
"ground_plane");
|
|
|
|
EXPECT_TRUE(filledSDF->HasElement("light"));
|
|
EXPECT_EQ(filledSDF->GetElement("light")->Get<std::string>("name"),
|
|
"sun");
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(WorldStateTest, OperatorsNoInsertionsDeletions)
|
|
{
|
|
// Load a world
|
|
this->Load("worlds/shapes.world", true);
|
|
physics::WorldPtr world = physics::get_world("default");
|
|
|
|
// Create a world state
|
|
physics::WorldState worldState0(world);
|
|
|
|
// Create the "next state"
|
|
// Only checking a case without insertions or deletions
|
|
std::ostringstream sdfStr;
|
|
sdfStr << "<sdf version ='" << SDF_VERSION << "'>"
|
|
<< "<world name='default'>"
|
|
<< "<state world_name='default'>"
|
|
<< "<model name='ground_plane'>"
|
|
<< " <pose>0 0 0 0 0 0</pose>"
|
|
<< "</model>"
|
|
<< "<model name='box'>"
|
|
<< " <pose>0 0 0.5 0 0 0</pose>"
|
|
<< "</model>"
|
|
<< "<model name='sphere'>"
|
|
<< " <pose>2 4 6 0 0 0</pose>"
|
|
<< "</model>"
|
|
<< "<model name='cylinder'>"
|
|
<< " <pose>3 6 9 0 1.5707 0</pose>"
|
|
<< "</model>"
|
|
<< "<light name='sun'>"
|
|
<< " <pose>10 20 30 0 0 0</pose>"
|
|
<< "</light>"
|
|
<< "</state>"
|
|
<< "</world>"
|
|
<< "</sdf>";
|
|
|
|
sdf::SDFPtr worldSDF(new sdf::SDF);
|
|
|
|
worldSDF->SetFromString(sdfStr.str());
|
|
EXPECT_TRUE(worldSDF->Root()->HasElement("world"));
|
|
|
|
auto worldElem = worldSDF->Root()->GetElement("world");
|
|
EXPECT_TRUE(worldElem->HasElement("state"));
|
|
|
|
physics::WorldState worldState1(worldElem->GetElement("state"));
|
|
|
|
// Check that the 2 states are of the same world, with same entities
|
|
EXPECT_EQ(worldState0.GetName(), worldState1.GetName());
|
|
EXPECT_EQ(worldState0.GetModelStateCount(), worldState1.GetModelStateCount());
|
|
EXPECT_EQ(worldState0.LightStateCount(), worldState1.LightStateCount());
|
|
|
|
// Check subtraction
|
|
auto worldState2 = worldState1 - worldState0;
|
|
EXPECT_FALSE(worldState2.IsZero());
|
|
|
|
// Check there were no deletions or insertions
|
|
auto deletions = worldState2.Deletions();
|
|
EXPECT_EQ(deletions.size(), 0u);
|
|
|
|
auto insertions = worldState2.Insertions();
|
|
EXPECT_EQ(insertions.size(), 0u);
|
|
|
|
// Check that entities which are the same in both states were removed
|
|
EXPECT_EQ(worldState2.GetModelStateCount(), 2u);
|
|
EXPECT_EQ(worldState2.LightStateCount(), 1u);
|
|
|
|
auto modelStates = worldState2.GetModelStates();
|
|
EXPECT_EQ(modelStates.size(), 2u);
|
|
EXPECT_EQ(modelStates["sphere"].GetPose(),
|
|
ignition::math::Pose3d(2, 2.5, 5.5, 0, 0, 0));
|
|
EXPECT_EQ(modelStates["cylinder"].GetPose(),
|
|
ignition::math::Pose3d(3, 7.5, 8.5, 0, 0, 0));
|
|
EXPECT_EQ(modelStates["fake_model"].GetPose(),
|
|
ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
|
|
|
|
auto lightStates = worldState2.LightStates();
|
|
EXPECT_EQ(lightStates.size(), 1u);
|
|
EXPECT_EQ(lightStates["sun"].Pose(),
|
|
ignition::math::Pose3d(10, 20, 20, 0, 0, 0));
|
|
EXPECT_EQ(lightStates["fake_light"].Pose(),
|
|
ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
|
|
|
|
// Check addition
|
|
worldState2 = worldState1 + worldState0;
|
|
EXPECT_FALSE(worldState2.IsZero());
|
|
|
|
// Check there were no deletions or insertions
|
|
deletions = worldState2.Deletions();
|
|
EXPECT_EQ(deletions.size(), 0u);
|
|
|
|
insertions = worldState2.Insertions();
|
|
EXPECT_EQ(insertions.size(), 0u);
|
|
|
|
// Check that all entities are present
|
|
EXPECT_EQ(worldState2.GetModelStateCount(), 4u);
|
|
EXPECT_EQ(worldState2.LightStateCount(), 1u);
|
|
|
|
modelStates = worldState2.GetModelStates();
|
|
EXPECT_EQ(modelStates.size(), 4u);
|
|
EXPECT_EQ(modelStates["ground_plane"].GetPose(),
|
|
ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
|
|
EXPECT_EQ(modelStates["box"].GetPose(),
|
|
ignition::math::Pose3d(0, 0, 1, 0, 0, 0));
|
|
EXPECT_EQ(modelStates["sphere"].GetPose(),
|
|
ignition::math::Pose3d(2, 5.5, 6.5, 0, 0, 0));
|
|
EXPECT_EQ(modelStates["cylinder"].GetPose().pos,
|
|
ignition::math::Vector3d(3, 4.5, 9.5));
|
|
|
|
lightStates = worldState2.LightStates();
|
|
EXPECT_EQ(lightStates.size(), 1u);
|
|
EXPECT_EQ(lightStates["sun"].Pose(),
|
|
ignition::math::Pose3d(10, 20, 40, 0, 0, 0));
|
|
|
|
// Copy by assignment
|
|
worldState1 = worldState0;
|
|
|
|
// Check states are equal
|
|
EXPECT_EQ(worldState0.GetName(), worldState1.GetName());
|
|
EXPECT_TRUE((worldState0 - worldState1).IsZero());
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(WorldStateTest, InsertionOfMeshModel)
|
|
{
|
|
// Load a world
|
|
this->Load("worlds/empty.world", true);
|
|
physics::WorldPtr world = physics::get_world("default");
|
|
|
|
// Create a world state
|
|
physics::WorldState worldState0(world);
|
|
|
|
// Insert a model with a scaled mesh
|
|
msgs::Factory msg;
|
|
std::ostringstream newModelStr;
|
|
|
|
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
|
|
<< "<model name ='test_model'>"
|
|
<< "<pose>2 2 2 0.1 0.1 0.1</pose>"
|
|
<< "<link name ='link'>"
|
|
<< " <collision name ='collision'>"
|
|
<< " <geometry>"
|
|
<< " <mesh>"
|
|
<< " <uri>" << std::string(PROJECT_SOURCE_PATH)
|
|
<< "/test/data/box_offset.dae</uri>"
|
|
<< " <scale>0.2 0.3 0.4</scale>"
|
|
<< " </mesh>"
|
|
<< " </geometry>"
|
|
<< " </collision>"
|
|
<< " <visual name ='visual'>"
|
|
<< " <geometry>"
|
|
<< " <mesh>"
|
|
<< " <uri>" << std::string(PROJECT_SOURCE_PATH)
|
|
<< "/test/data/box_offset.dae</uri>"
|
|
<< " <scale>0.5 0.6 0.7</scale>"
|
|
<< " </mesh>"
|
|
<< " </geometry>"
|
|
<< " </visual>"
|
|
<< "</link>"
|
|
<< "</model>"
|
|
<< "</sdf>";
|
|
|
|
msg.set_sdf(newModelStr.str());
|
|
this->factoryPub->Publish(msg);
|
|
|
|
this->WaitUntilEntitySpawn("test_model", 100, 100);
|
|
ASSERT_TRUE(world->GetModel("test_model") != nullptr);
|
|
|
|
// Create a new world state
|
|
physics::WorldState worldState1(world);
|
|
|
|
// Check that the 2nd state has more entities
|
|
EXPECT_EQ(worldState0.GetModelStateCount(), 1u);
|
|
EXPECT_EQ(worldState1.GetModelStateCount(), 2u);
|
|
|
|
// Difference between states to generate insertion
|
|
auto worldState2 = worldState1 - worldState0;
|
|
EXPECT_FALSE(worldState2.IsZero());
|
|
|
|
// Check there were no deletions
|
|
auto deletions = worldState2.Deletions();
|
|
EXPECT_EQ(deletions.size(), 0u);
|
|
|
|
// Check the old entity disappears and the new entity appears as insertion
|
|
EXPECT_EQ(worldState2.GetModelStateCount(), 0u);
|
|
auto insertions = worldState2.Insertions();
|
|
EXPECT_EQ(insertions.size(), 1u);
|
|
|
|
// Check that inserted entity's scale wasn't changed
|
|
auto inserted = insertions[0];
|
|
EXPECT_TRUE(inserted.find("<scale>0.2 0.3 0.4</scale>") != std::string::npos);
|
|
EXPECT_TRUE(inserted.find("<scale>0.5 0.6 0.7</scale>") != std::string::npos);
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(WorldStateTest, Times)
|
|
{
|
|
// Load a world
|
|
this->Load("worlds/empty.world", true);
|
|
physics::WorldPtr world = physics::get_world("default");
|
|
|
|
// Create the world state
|
|
physics::WorldState worldState(world);
|
|
|
|
// Check default times
|
|
EXPECT_EQ(worldState.GetSimTime(), common::Time(0));
|
|
EXPECT_GT(worldState.GetWallTime(), common::Time(0));
|
|
EXPECT_EQ(worldState.GetRealTime(), common::Time(0));
|
|
|
|
// Set times
|
|
worldState.SetSimTime(common::Time(1));
|
|
worldState.SetWallTime(common::Time(2));
|
|
worldState.SetRealTime(common::Time(3));
|
|
|
|
// Check times
|
|
EXPECT_EQ(worldState.GetSimTime(), common::Time(1));
|
|
EXPECT_EQ(worldState.GetWallTime(), common::Time(2));
|
|
EXPECT_EQ(worldState.GetRealTime(), common::Time(3));
|
|
}
|