244 lines
7.2 KiB
C++
244 lines
7.2 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/msgs/msgs.hh"
|
|
#include "gazebo/physics/PhysicsTypes.hh"
|
|
#include "gazebo/physics/Light.hh"
|
|
#include "gazebo/physics/LightState.hh"
|
|
|
|
using namespace gazebo;
|
|
|
|
class LightStateTest : public ServerFixture { };
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(LightStateTest, SDFConstructor)
|
|
{
|
|
// Create the state sdf
|
|
std::ostringstream sdfStr;
|
|
sdfStr << "<sdf version ='" << SDF_VERSION << "'>"
|
|
<< "<world name='default'>"
|
|
<< "<state world_name='default'>"
|
|
<< "<light name='light_0'>"
|
|
<< " <pose>0 0 0.5 0 0 0</pose>"
|
|
<< "</light>"
|
|
<< "</state>"
|
|
<< "</world>"
|
|
<< "</sdf>";
|
|
|
|
sdf::SDFPtr worldSDF(new sdf::SDF);
|
|
|
|
worldSDF->SetFromString(sdfStr.str());
|
|
EXPECT_TRUE(worldSDF->Root()->HasElement("world"));
|
|
|
|
sdf::ElementPtr worldElem = worldSDF->Root()->GetElement("world");
|
|
EXPECT_TRUE(worldElem->HasElement("state"));
|
|
|
|
sdf::ElementPtr stateElem = worldElem->GetElement("state");
|
|
EXPECT_TRUE(stateElem->HasElement("light"));
|
|
|
|
// Create the light state
|
|
physics::LightState lightState(stateElem->GetElement("light"));
|
|
|
|
// Check light state against values from the sdf string
|
|
EXPECT_EQ(lightState.Pose(), ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0));
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(LightStateTest, LightConstructor)
|
|
{
|
|
ignition::math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3);
|
|
|
|
// Load a world
|
|
this->Load("worlds/empty.world", true);
|
|
physics::WorldPtr world = physics::get_world("default");
|
|
|
|
// Create a base to be the light's parent
|
|
physics::BasePtr basePtr;
|
|
basePtr.reset(new physics::Base(physics::BasePtr()));
|
|
basePtr->SetName("world_root_element");
|
|
basePtr->SetWorld(world);
|
|
|
|
// Create the light message
|
|
msgs::Light lightMsg;
|
|
lightMsg.set_name("test_light");
|
|
msgs::Set(lightMsg.mutable_pose(), pose);
|
|
|
|
// Create the light
|
|
physics::LightPtr lightPtr(new physics::Light(basePtr));
|
|
EXPECT_TRUE(lightPtr != NULL);
|
|
|
|
lightPtr->ProcessMsg(lightMsg);
|
|
EXPECT_EQ(lightPtr->GetWorldPose(), pose);
|
|
|
|
// Create the light state
|
|
physics::LightState lightState(lightPtr, common::Time(1), common::Time(1), 1);
|
|
|
|
// Check light state against values from the message
|
|
EXPECT_EQ(lightState.Pose(), pose);
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(LightStateTest, FillSDF)
|
|
{
|
|
// Create the state sdf
|
|
std::ostringstream sdfStr;
|
|
sdfStr << "<sdf version ='" << SDF_VERSION << "'>"
|
|
<< "<world name='default'>"
|
|
<< "<state world_name='default'>"
|
|
<< "<light name='light_0'>"
|
|
<< " <pose>1 2 3 0.1 0.2 0.3</pose>"
|
|
<< "</light>"
|
|
<< "</state>"
|
|
<< "</world>"
|
|
<< "</sdf>";
|
|
|
|
sdf::SDFPtr worldSDF(new sdf::SDF);
|
|
|
|
worldSDF->SetFromString(sdfStr.str());
|
|
EXPECT_TRUE(worldSDF->Root()->HasElement("world"));
|
|
|
|
sdf::ElementPtr worldElem = worldSDF->Root()->GetElement("world");
|
|
EXPECT_TRUE(worldElem->HasElement("state"));
|
|
|
|
sdf::ElementPtr stateElem = worldElem->GetElement("state");
|
|
EXPECT_TRUE(stateElem->HasElement("light"));
|
|
|
|
sdf::ElementPtr lightElem = stateElem->GetElement("light");
|
|
|
|
// Create the light state
|
|
physics::LightState lightState(lightElem);
|
|
|
|
// Fill SDF
|
|
sdf::ElementPtr filledSDF(new sdf::Element);
|
|
sdf::initFile("light_state.sdf", filledSDF);
|
|
lightState.FillSDF(filledSDF);
|
|
|
|
EXPECT_TRUE(filledSDF->HasAttribute("name"));
|
|
EXPECT_EQ(filledSDF->GetName(), lightElem->GetName());
|
|
EXPECT_TRUE(filledSDF->HasElement("pose"));
|
|
EXPECT_EQ(filledSDF->GetElement("pose")->Get<ignition::math::Pose3d>(),
|
|
lightElem->GetElement("pose")->Get<ignition::math::Pose3d>());
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(LightStateTest, Operators)
|
|
{
|
|
// Create the state sdf
|
|
std::ostringstream sdfStr;
|
|
sdfStr << "<sdf version ='" << SDF_VERSION << "'>"
|
|
<< "<world name='default'>"
|
|
<< "<state world_name='default'>"
|
|
<< "<light name='light_0'>"
|
|
<< " <pose>1 2 3 0 0 0</pose>"
|
|
<< "</light>"
|
|
<< "<light name='light_1'>"
|
|
<< " <pose>4 5 6 0 0 0</pose>"
|
|
<< "</light>"
|
|
<< "</state>"
|
|
<< "</world>"
|
|
<< "</sdf>";
|
|
|
|
sdf::SDFPtr worldSDF(new sdf::SDF);
|
|
|
|
worldSDF->SetFromString(sdfStr.str());
|
|
EXPECT_TRUE(worldSDF->Root()->HasElement("world"));
|
|
|
|
sdf::ElementPtr worldElem = worldSDF->Root()->GetElement("world");
|
|
EXPECT_TRUE(worldElem->HasElement("state"));
|
|
|
|
sdf::ElementPtr stateElem = worldElem->GetElement("state");
|
|
EXPECT_TRUE(stateElem->HasElement("light"));
|
|
|
|
sdf::ElementPtr light0 = stateElem->GetElement("light");
|
|
sdf::ElementPtr light1 = light0->GetNextElement("light");
|
|
|
|
// Create light states
|
|
physics::LightState lightState0(light0);
|
|
physics::LightState lightState1(light1);
|
|
|
|
// Check states are different
|
|
EXPECT_NE(lightState0.GetName(), lightState1.GetName());
|
|
EXPECT_NE(lightState0.Pose(), lightState1.Pose());
|
|
EXPECT_FALSE((lightState0 - lightState1).IsZero());
|
|
|
|
// Check subtraction
|
|
auto lightState2 = lightState0 - lightState1;
|
|
EXPECT_EQ(lightState2.Pose(), ignition::math::Pose3d(-3, -3, -3, 0, 0, 0));
|
|
|
|
lightState2 = lightState1 - lightState0;
|
|
EXPECT_EQ(lightState2.Pose(), ignition::math::Pose3d(3, 3, 3, 0, 0, 0));
|
|
|
|
// Check addition
|
|
lightState2 = lightState1 + lightState0;
|
|
EXPECT_EQ(lightState2.Pose(), ignition::math::Pose3d(5, 7, 9, 0, 0, 0));
|
|
|
|
// Copy by assignment
|
|
lightState1 = lightState0;
|
|
|
|
// Check states are equal
|
|
EXPECT_EQ(lightState0.GetName(), lightState1.GetName());
|
|
EXPECT_EQ(lightState0.Pose(), lightState1.Pose());
|
|
EXPECT_TRUE((lightState0 - lightState1).IsZero());
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(LightStateTest, Print)
|
|
{
|
|
// Create the state sdf
|
|
std::ostringstream sdfStr;
|
|
sdfStr << "<sdf version ='" << SDF_VERSION << "'>"
|
|
<< "<world name='default'>"
|
|
<< "<state world_name='default'>"
|
|
<< "<light name='light_0'>"
|
|
<< " <pose>1 2 3 0.1 0.2 0.3</pose>"
|
|
<< "</light>"
|
|
<< "</state>"
|
|
<< "</world>"
|
|
<< "</sdf>";
|
|
|
|
sdf::SDFPtr worldSDF(new sdf::SDF);
|
|
|
|
worldSDF->SetFromString(sdfStr.str());
|
|
EXPECT_TRUE(worldSDF->Root()->HasElement("world"));
|
|
|
|
sdf::ElementPtr worldElem = worldSDF->Root()->GetElement("world");
|
|
EXPECT_TRUE(worldElem->HasElement("state"));
|
|
|
|
sdf::ElementPtr stateElem = worldElem->GetElement("state");
|
|
EXPECT_TRUE(stateElem->HasElement("light"));
|
|
|
|
// Create the light state
|
|
physics::LightState lightState(stateElem->GetElement("light"));
|
|
|
|
// Stream
|
|
std::ostringstream strOut;
|
|
strOut << lightState;
|
|
|
|
EXPECT_STREQ(strOut.str().c_str(),
|
|
"<light name='light_0'><pose>1.000 2.000 3.000 0.100 0.200 0.300 "\
|
|
"</pose></light>");
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
::testing::InitGoogleTest(&argc, argv);
|
|
return RUN_ALL_TESTS();
|
|
}
|
|
|