/* * 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 #include #include "gazebo/test/ServerFixture.hh" #include "gazebo/test/helper_physics_generator.hh" #include "gazebo/sensors/AltimeterSensor.hh" #define TOL 1e-4 using namespace gazebo; /// \brief Test class for the altimeter sensor class AltimeterSensor_TEST : public ServerFixture, public testing::WithParamInterface { /// \brief Check that a simple altimeter works correctly /// The default should be inserted with altitude = 0 and velocity = 0 /// \param[in] _physicsEngine The type of physics engine to use. public: void BasicAltimeterSensorCheck(const std::string &_physicsEngine); /// \brief Check that an altimeter at a non-zero altitude works. /// \param[in] _physicsEngine The type of physics engine to use. public: void NonzeroAltimeterSensorCheck(const std::string &_physicsEngine); /// \brief Check that a falling altimeter has the correct velocity. /// \param[in] _physicsEngine The type of physics engine to use. public: void LinearAltimeterSensorCheck(const std::string &_physicsEngine); /// \brief Check that a rotating altimeter has the correct velocity. /// \param[in] _physicsEngine The type of physics engine to use. public: void AngularAltimeterSensorCheck(const std::string &_physicsEngine); /// \brief Check that a rotating and falling altimeter has the correct /// velocity. /// \param[in] _physicsEngine The type of physics engine to use. public: void LinearAngularAltimeterSensorCheck( const std::string &_physicsEngine); }; // An altitude sensor static std::string altSensorString = "" " " " 1" " 10.0" " " " " " " ""; ///////////////////////////////////////////////// void AltimeterSensor_TEST::BasicAltimeterSensorCheck( const std::string &_physicsEngine) { Load("worlds/empty.world", false, _physicsEngine); sensors::SensorManager *mgr = sensors::SensorManager::Instance(); physics::WorldPtr world = physics::get_world("default"); sdf::ElementPtr sdf(new sdf::Element); sdf::initFile("sensor.sdf", sdf); sdf::readString(altSensorString, sdf); // Create the altimeter sensor std::string sensorName = mgr->CreateSensor(sdf, "default", "ground_plane::link", 0); // Make sure the returned sensor name is correct EXPECT_EQ(sensorName, std::string("default::ground_plane::link::altimeter")); // Update the sensor manager so that it can process new sensors. mgr->Update(); // Get a pointer to the altimeter sensor sensors::AltimeterSensorPtr sensor = std::dynamic_pointer_cast (mgr->GetSensor(sensorName)); // Make sure the above dynamic cast worked. EXPECT_TRUE(sensor != NULL); // By default the altitude of the sensor should be zero EXPECT_DOUBLE_EQ(sensor->Altitude(), 0.0); EXPECT_DOUBLE_EQ(sensor->VerticalVelocity(), 0.0); } ///////////////////////////////////////////////// // Check linear velocity is correct void AltimeterSensor_TEST::LinearAltimeterSensorCheck( const std::string &_physicsEngine) { Load("worlds/empty.world", true, _physicsEngine); physics::WorldPtr world = physics::get_world("default"); ASSERT_TRUE(world != NULL); // Verify physics engine type physics::PhysicsEnginePtr physics = world->GetPhysicsEngine(); ASSERT_TRUE(physics != NULL); EXPECT_EQ(physics->GetType(), _physicsEngine); // Spawn an altimeter std::string modelName = "altModel"; std::string altSensorName = "altSensor"; ignition::math::Pose3d modelPose(0, 0, 10, 0, 0, 0); std::string topic = "~/" + altSensorName + "_" + _physicsEngine; SpawnUnitAltimeterSensor(modelName, altSensorName, "box", topic, modelPose.Pos(), modelPose.Rot().Euler()); // Get the altimeter sensors::SensorPtr sensor = sensors::get_sensor("altSensor"); sensors::AltimeterSensorPtr altSensor = std::dynamic_pointer_cast(sensor); ASSERT_TRUE(altSensor != NULL); sensors::SensorManager::Instance()->Init(); altSensor->SetActive(true); int steps = 10; world->Step(steps); altSensor->Update(true); // The altimeter should have a velocity of v = g * dt EXPECT_FLOAT_EQ(altSensor->VerticalVelocity(), physics->GetGravity().z * (physics->GetMaxStepSize()*steps)); } ///////////////////////////////////////////////// // Check rotational velocity is correct void AltimeterSensor_TEST::AngularAltimeterSensorCheck( const std::string &_physicsEngine) { Load("worlds/test_altimeter_rotation.world", true, _physicsEngine); physics::WorldPtr world = physics::get_world("default"); ASSERT_TRUE(world != NULL); // Verify physics engine type physics::PhysicsEnginePtr physics = world->GetPhysicsEngine(); ASSERT_TRUE(physics != NULL); EXPECT_EQ(physics->GetType(), _physicsEngine); physics::ModelPtr model = world->GetModel("model"); ASSERT_TRUE(model != NULL); physics::JointPtr joint = model->GetJoint("joint"); ASSERT_TRUE(joint != NULL); sensors::SensorPtr sensor = sensors::get_sensor("altimeter"); sensors::AltimeterSensorPtr altSensor = std::dynamic_pointer_cast(sensor); ASSERT_TRUE(altSensor != NULL); sensors::SensorManager::Instance()->Init(); altSensor->SetActive(true); int steps = 1; world->Step(steps); altSensor->Update(true); // Get the link's angular velocity ignition::math::Vector3d avel = model->GetLink("link")->GetRelativeAngularVel().Ign(); // Expect the altimeter's velocity to equal the angular velocity at the // end of the rod. EXPECT_NEAR(altSensor->VerticalVelocity(), avel.Sum() * 10, 1e-3); } ///////////////////////////////////////////////// // Check angular and linear velocity is correct void AltimeterSensor_TEST::LinearAngularAltimeterSensorCheck( const std::string &_physicsEngine) { Load("worlds/test_altimeter_linear_angular.world", true, _physicsEngine); physics::WorldPtr world = physics::get_world("default"); ASSERT_TRUE(world != NULL); // Verify physics engine type physics::PhysicsEnginePtr physics = world->GetPhysicsEngine(); ASSERT_TRUE(physics != NULL); EXPECT_EQ(physics->GetType(), _physicsEngine); physics::ModelPtr model = world->GetModel("model"); ASSERT_TRUE(model != NULL); physics::JointPtr joint = model->GetJoint("joint"); ASSERT_TRUE(joint != NULL); sensors::SensorPtr sensor = sensors::get_sensor("altimeter"); sensors::AltimeterSensorPtr altSensor = std::dynamic_pointer_cast(sensor); ASSERT_TRUE(altSensor != NULL); sensors::SensorManager::Instance()->Init(); altSensor->SetActive(true); int steps = 10; world->Step(steps); altSensor->Update(true); // Angular velocity of the rod ignition::math::Vector3d avel = model->GetLink("link")->GetRelativeAngularVel().Ign(); // Linear velocity of the rod at the location that is attached to // the prismatic joint. ignition::math::Vector3d lvel = model->GetLink("link")->GetWorldLinearVel( ignition::math::Vector3d(0, -5, 0)).Ign(); // Expect the altimeter's velocity to equal the angular velocity at the // end of the rod + the rod's linear velocity. EXPECT_NEAR(altSensor->VerticalVelocity(), avel.Sum() * 10 + lvel.Z(), 1e-4); } ///////////////////////////////////////////////// // If inserted at X=0,Y=0,Z=10m void AltimeterSensor_TEST::NonzeroAltimeterSensorCheck( const std::string &_physicsEngine) { Load("worlds/empty.world", true, _physicsEngine); physics::WorldPtr world = physics::get_world("default"); ASSERT_TRUE(world != NULL); // Verify physics engine type physics::PhysicsEnginePtr physics = world->GetPhysicsEngine(); ASSERT_TRUE(physics != NULL); EXPECT_EQ(physics->GetType(), _physicsEngine); // Spawn an altimeter sensor at a height of 10m std::string modelName = "altModel"; std::string altSensorName = "altSensor"; ignition::math::Pose3d modelPose(0, 0, 10, 0, 0, 0); std::string topic = "~/" + altSensorName + "_" + _physicsEngine; SpawnUnitAltimeterSensor(modelName, altSensorName, "box", topic, modelPose.Pos(), modelPose.Rot().Euler()); sensors::SensorPtr sensor = sensors::get_sensor(altSensorName); sensors::AltimeterSensorPtr altSensor = std::dynamic_pointer_cast(sensor); ASSERT_TRUE(altSensor != NULL); sensors::SensorManager::Instance()->Init(); altSensor->SetActive(true); // Check for match EXPECT_DOUBLE_EQ(altSensor->ReferenceAltitude(), 10.0); EXPECT_DOUBLE_EQ(altSensor->Altitude(), 0.0); EXPECT_DOUBLE_EQ(altSensor->VerticalVelocity(), 0.0); } ///////////////////////////////////////////////// TEST_P(AltimeterSensor_TEST, BasicAltimeterSensorCheck) { BasicAltimeterSensorCheck(GetParam()); } ///////////////////////////////////////////////// TEST_P(AltimeterSensor_TEST, LinearAltimeterSensorCheck) { LinearAltimeterSensorCheck(GetParam()); } ///////////////////////////////////////////////// TEST_P(AltimeterSensor_TEST, AngularAltimeterSensorCheck) { AngularAltimeterSensorCheck(GetParam()); } ///////////////////////////////////////////////// TEST_P(AltimeterSensor_TEST, LinearAngularAltimeterSensorCheck) { LinearAngularAltimeterSensorCheck(GetParam()); } ///////////////////////////////////////////////// TEST_P(AltimeterSensor_TEST, NonzeroAltimeterSensorCheck) { NonzeroAltimeterSensorCheck(GetParam()); } INSTANTIATE_TEST_CASE_P(PhysicsEngines, AltimeterSensor_TEST, PHYSICS_ENGINE_VALUES); ///////////////////////////////////////////////// int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }