diff --git a/CMakeLists.txt b/CMakeLists.txt index 94162ecd4a..859f59c740 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,7 +50,7 @@ set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # as protobuf could be find transitively by any dependency set(protobuf_MODULE_COMPATIBLE TRUE) -ign_find_package(sdformat12 REQUIRED) +ign_find_package(sdformat12 REQUIRED VERSION 12.2) set(SDF_VER ${sdformat12_VERSION_MAJOR}) #-------------------------------------- diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index e2ca63bfc1..9f8b9286c2 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -26,12 +26,37 @@ #include #include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/components/AirPressureSensor.hh" +#include "ignition/gazebo/components/Altimeter.hh" +#include "ignition/gazebo/components/Camera.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/DepthCamera.hh" +#include "ignition/gazebo/components/ForceTorque.hh" +#include "ignition/gazebo/components/GpuLidar.hh" +#include "ignition/gazebo/components/Imu.hh" +#include "ignition/gazebo/components/Inertial.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointType.hh" #include "ignition/gazebo/components/Light.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/LogicalCamera.hh" +#include "ignition/gazebo/components/Magnetometer.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/RgbdCamera.hh" +#include "ignition/gazebo/components/SelfCollide.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/components/SegmentationCamera.hh" +#include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/ThermalCamera.hh" +#include "ignition/gazebo/components/ThreadPitch.hh" +#include "ignition/gazebo/components/WindMode.hh" #include "ignition/gazebo/components/World.hh" @@ -277,7 +302,7 @@ namespace sdf_generator // First remove child entities of whose names can be changed during // simulation (eg. models). Then we add them back from the data in the // ECM. - // TODO(addisu) Remove actors and lights + // TODO(addisu) Remove actors std::vector toRemove; if (_elem->HasElement("model")) { @@ -287,6 +312,15 @@ namespace sdf_generator toRemove.push_back(modelElem); } } + if (_elem->HasElement("light")) + { + for (auto lightElem = _elem->GetElement("light"); lightElem; + lightElem = lightElem->GetNextElement("light")) + { + toRemove.push_back(lightElem); + } + } + for (const auto &e : toRemove) { _elem->RemoveChild(e); @@ -294,6 +328,7 @@ namespace sdf_generator auto worldDir = common::parentPath(worldSdf->Data().Element()->FilePath()); + // models _ecm.Each( [&](const Entity &_modelEntity, const components::Model *, const components::ModelSdf *_modelSdf) @@ -386,6 +421,21 @@ namespace sdf_generator return true; }); + // lights + _ecm.Each( + [&](const Entity &_lightEntity, + const components::Light *, + const components::ParentEntity *_parent) -> bool + { + if (_parent->Data() != _entity) + return true; + + auto lightElem = _elem->AddElement("light"); + updateLightElement(lightElem, _ecm, _lightEntity); + + return true; + }); + return true; } @@ -397,10 +447,12 @@ namespace sdf_generator if (!copySdf(_ecm.Component(_entity), _elem)) return false; - // Update sdf based current components. Here are the list of components to - // be updated: + // Update sdf based on current components. Here are the list of components + // to be updated: // - Name // - Pose + // - Static + // - SelfCollide // This list is to be updated as other components become updateable during // simulation auto *nameComp = _ecm.Component(_entity); @@ -421,19 +473,463 @@ namespace sdf_generator } poseElem->Set(poseComp->Data()); + // static + auto *staticComp = _ecm.Component(_entity); + if (staticComp) + _elem->GetElement("static")->Set(staticComp->Data()); + + // self collide + auto *selfCollideComp = _ecm.Component(_entity); + if (selfCollideComp) + _elem->GetElement("self_collide")->Set(selfCollideComp->Data()); + const auto *pathComp = _ecm.Component(_entity); - if (_elem->HasElement("link") && nullptr != pathComp) + if (_elem->HasElement("link")) + { + if (nullptr != pathComp) + { + // Update relative URIs to use absolute paths. Relative URIs work fine + // in included models, but they have to be converted to absolute URIs + // when the included model is expanded. + relativeToAbsoluteUri(_elem, common::parentPath(pathComp->Data())); + } + + // update links + sdf::ElementPtr linkElem = _elem->GetElement("link"); + while (linkElem) + { + std::string linkName = linkElem->Get("name"); + auto linkEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(linkName)); + if (linkEnt != kNullEntity) + updateLinkElement(linkElem, _ecm, linkEnt); + linkElem = linkElem->GetNextElement("link"); + } + } + + if (_elem->HasElement("joint")) + { + // update joints + sdf::ElementPtr jointElem = _elem->GetElement("joint"); + while (jointElem) + { + std::string jointName = jointElem->Get("name"); + auto jointEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(jointName)); + if (jointEnt != kNullEntity) + updateJointElement(jointElem, _ecm, jointEnt); + jointElem = jointElem->GetNextElement("joint"); + } + } + + return true; + } + + ///////////////////////////////////////////////// + bool updateLinkElement(const sdf::ElementPtr &_elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on current components. Here are the list of components + // to be updated: + // - Name + // - Pose + // - Inertial + // - WindMode + // This list is to be updated as other components become updateable during + // simulation + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + + // inertial + auto inertialComp = _ecm.Component(_entity); + if (inertialComp) { - // Update relative URIs to use absolute paths. Relative URIs work fine in - // included models, but they have to be converted to absolute URIs when - // the included model is expanded. - relativeToAbsoluteUri(_elem, common::parentPath(pathComp->Data())); + math::Inertiald inertial = inertialComp->Data(); + sdf::ElementPtr inertialElem = _elem->GetElement("inertial"); + inertialElem->GetElement("pose")->Set(inertial.Pose()); + const math::MassMatrix3d &massMatrix = inertial.MassMatrix(); + inertialElem->GetElement("mass")->Set(massMatrix.Mass()); + sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); + inertiaElem->GetElement("ixx")->Set(massMatrix.Ixx()); + inertiaElem->GetElement("ixy")->Set(massMatrix.Ixy()); + inertiaElem->GetElement("ixz")->Set(massMatrix.Ixz()); + inertiaElem->GetElement("iyy")->Set(massMatrix.Iyy()); + inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz()); + inertiaElem->GetElement("izz")->Set(massMatrix.Izz()); } + + // wind mode + auto windModeComp = _ecm.Component(_entity); + if (windModeComp) + { + bool windMode = windModeComp->Data(); + _elem->GetElement("enable_wind")->Set(windMode); + } + + // update sensors + if (_elem->HasElement("sensor")) + { + sdf::ElementPtr sensorElem = _elem->GetElement("sensor"); + while (sensorElem) + { + std::string sensorName = sensorElem->Get("name"); + auto sensorEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(sensorName)); + if (sensorEnt != kNullEntity) + updateSensorElement(sensorElem, _ecm, sensorEnt); + sensorElem = sensorElem->GetNextElement("sensor"); + } + } + + // update lights + if (_elem->HasElement("light")) + { + sdf::ElementPtr lightElem = _elem->GetElement("light"); + while (lightElem) + { + std::string lightName = lightElem->Get("name"); + auto lightEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(lightName)); + if (lightEnt != kNullEntity) + updateLightElement(lightElem, _ecm, lightEnt); + lightElem = lightElem->GetNextElement("light"); + } + } + return true; } + ///////////////////////////////////////////////// + bool updateSensorElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on current components. + // This list is to be updated as other components become updateable during + // simulation + auto updateSensorNameAndPose = [&] + { + // override name and pose sdf element using values from ECM + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + return true; + }; + + // camera + auto camComp = _ecm.Component(_entity); + if (camComp) + { + const sdf::Sensor &sensor = camComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // depth camera + auto depthCamComp = _ecm.Component(_entity); + if (depthCamComp) + { + const sdf::Sensor &sensor = depthCamComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // thermal camera + auto thermalCamComp = _ecm.Component(_entity); + if (thermalCamComp) + { + const sdf::Sensor &sensor = thermalCamComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // logical camera + auto logicalCamComp = _ecm.Component(_entity); + if (logicalCamComp) + { + // components::LogicalCamera holds an sdf::ElementPtr instead of an + // sdf::Sensor + _elem = logicalCamComp->Data(); + return updateSensorNameAndPose(); + } + // segmentation camera + auto segmentationCamComp = + _ecm.Component(_entity); + if (segmentationCamComp) + { + const sdf::Sensor &sensor = segmentationCamComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + + // gpu lidar + auto gpuLidarComp = _ecm.Component(_entity); + if (gpuLidarComp) + { + const sdf::Sensor &sensor = gpuLidarComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // altimeter + auto altimeterComp = _ecm.Component(_entity); + if (altimeterComp) + { + const sdf::Sensor &sensor = altimeterComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // contact + auto contactComp = _ecm.Component(_entity); + if (contactComp) + { + // components::ContactSensor holds an sdf::ElementPtr instead of an + // sdf::Sensor + _elem = contactComp->Data(); + return updateSensorNameAndPose(); + } + // air pressure + auto airPressureComp = + _ecm.Component(_entity); + if (airPressureComp) + { + const sdf::Sensor &sensor = airPressureComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // force torque + auto forceTorqueComp = _ecm.Component(_entity); + if (forceTorqueComp) + { + const sdf::Sensor &sensor = forceTorqueComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // imu + auto imuComp = _ecm.Component(_entity); + if (imuComp) + { + const sdf::Sensor &sensor = imuComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // magnetometer + auto magnetometerComp = + _ecm.Component(_entity); + if (magnetometerComp) + { + const sdf::Sensor &sensor = magnetometerComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + + return true; + } + + ///////////////////////////////////////////////// + bool updateLightElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on the light component + auto updateLightNameAndPose = [&] + { + // override name and pose sdf element using values from ECM + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + return true; + }; + + // light + auto lightComp = _ecm.Component(_entity); + if (lightComp) + { + const sdf::Light &light = lightComp->Data(); + _elem->Copy(light.ToElement()); + return updateLightNameAndPose(); + } + return true; + } + + ///////////////////////////////////////////////// + bool updateJointElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on the joint component + auto updateJointNameAndPose = [&] + { + // override name and pose sdf element using values from ECM + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + return true; + }; + + // joint + auto jointComp = _ecm.Component(_entity); + if (!jointComp) + { + return false; + } + + // joint type + auto jointTypeComp = _ecm.Component(_entity); + sdf::JointType jointType = jointTypeComp->Data(); + if (jointTypeComp) + { + std::string jointTypeStr = "invalid"; + switch (jointType) + { + case sdf::JointType::BALL: + jointTypeStr = "ball"; + break; + case sdf::JointType::CONTINUOUS: + jointTypeStr = "continuous"; + break; + case sdf::JointType::FIXED: + jointTypeStr = "fixed"; + break; + case sdf::JointType::PRISMATIC: + jointTypeStr = "prismatic"; + break; + case sdf::JointType::GEARBOX: + jointTypeStr = "gearbox"; + break; + case sdf::JointType::REVOLUTE: + jointTypeStr = "revolute"; + break; + case sdf::JointType::REVOLUTE2: + jointTypeStr = "revolute2"; + break; + case sdf::JointType::SCREW: + jointTypeStr = "screw"; + break; + case sdf::JointType::UNIVERSAL: + jointTypeStr = "universal"; + break; + default: + break; + } + _elem->GetAttribute("type")->Set(jointTypeStr); + } + + // parent + auto parentLinkNameComp = + _ecm.Component(_entity); + if (parentLinkNameComp) + { + _elem->GetElement("parent")->Set(parentLinkNameComp->Data()); + } + // child + auto childLinkNameComp = _ecm.Component(_entity); + if (childLinkNameComp) + { + _elem->GetElement("child")->Set(childLinkNameComp->Data()); + } + // thread pitch + auto threadPitchComp = _ecm.Component(_entity); + if (threadPitchComp && jointType == sdf::JointType::SCREW) + { + _elem->GetElement("thread_pitch")->Set(threadPitchComp->Data()); + } + // axis + auto jointAxisComp = _ecm.Component(_entity); + if (jointAxisComp) + { + const sdf::JointAxis axis = jointAxisComp->Data(); + _elem->GetElement("axis")->Copy(axis.ToElement()); + } + // axis2 + auto jointAxis2Comp = _ecm.Component(_entity); + if (jointAxis2Comp) + { + const sdf::JointAxis axis2 = jointAxis2Comp->Data(); + _elem->GetElement("axis2")->Copy(axis2.ToElement(1u)); + } + + // sensors + // remove existing ones in sdf element and add new ones from ECM. + std::vector toRemove; + if (_elem->HasElement("sensor")) + { + for (auto sensorElem = _elem->GetElement("sensor"); sensorElem; + sensorElem = sensorElem->GetNextElement("sensor")) + { + toRemove.push_back(sensorElem); + } + } + for (const auto &e : toRemove) + { + _elem->RemoveChild(e); + } + + auto sensorEntities = _ecm.EntitiesByComponents( + components::ParentEntity(_entity), components::Sensor()); + + for (const auto &sensorEnt : sensorEntities) + { + sdf::ElementPtr sensorElem = _elem->AddElement("sensor"); + updateSensorElement(sensorElem, _ecm, sensorEnt); + } + + return updateJointNameAndPose(); + } + ///////////////////////////////////////////////// /// \brief Checks if a string is a number /// \param[in] _str The string to check diff --git a/src/SdfGenerator.hh b/src/SdfGenerator.hh index e580fe53e8..83a7844c29 100644 --- a/src/SdfGenerator.hh +++ b/src/SdfGenerator.hh @@ -99,6 +99,49 @@ namespace sdf_generator const EntityComponentManager &_ecm, const Entity &_entity, const std::string &_uri); + /// \brief Update an sdf::Element of a link. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity Link entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateLinkElement(const sdf::ElementPtr &_elem, + const EntityComponentManager &_ecm, + const Entity &_entity); + + /// \brief Update an sdf::Element of a sensor. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity Sensor entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateSensorElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity); + + /// \brief Update an sdf::Element of a light. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity Light entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateLightElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity); + + /// \brief Update an sdf::Element of a joint. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity joint entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateJointElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity); } // namespace sdf_generator } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE } // namespace gazebo diff --git a/src/SdfGenerator_TEST.cc b/src/SdfGenerator_TEST.cc index aace52588f..3599fda3ef 100644 --- a/src/SdfGenerator_TEST.cc +++ b/src/SdfGenerator_TEST.cc @@ -117,6 +117,43 @@ static testing::AssertionResult isSubset(const sdf::ElementPtr &_elemA, << "'"; } } + else if (valA->GetTypeName() == "double") + { + double dblA, dblB; + valA->Get(dblA); + valB->Get(dblB); + if (!math::equal(dblA, dblB)) + { + return testing::AssertionFailure() + << "Mismatch in value as double: '" << dblA << "' vs '" + << dblB << "'"; + } + } + else if (valA->GetTypeName() == "float") + { + float fltA, fltB; + valA->Get(fltA); + valB->Get(fltB); + if (!math::equal(fltA, fltB)) + { + return testing::AssertionFailure() + << "Mismatch in value as float: '" << fltA << "' vs '" + << fltB << "'"; + } + } + else if (valA->GetTypeName() == "bool") + { + bool boolA, boolB; + valA->Get(boolA); + valB->Get(boolB); + if (boolA != boolB) + { + return testing::AssertionFailure() + << "Mismatch in value as bool: '" << boolA << "' vs '" + << boolB << "'"; + } + + } else if (valA->GetAsString() != valB->GetAsString()) { return testing::AssertionFailure() @@ -148,6 +185,27 @@ static testing::AssertionResult isSubset(const sdf::ElementPtr &_elemA, if (!result) { + // Ignore missing pose values if the pose is zero. + sdf::ParamPtr childValA = childElemA->GetValue(); + if (childValA->GetTypeName() == "pose") + { + math::Pose3d childValPose; + childValA->Get(childValPose); + if (childValPose == math::Pose3d::Zero) + return testing::AssertionSuccess(); + } + else if (childValA->GetTypeName() == "bool") + { + bool childValBool; + childValA->Get(childValBool); + if (!childValBool && (childElemA->GetName() == "static" || + childElemA->GetName() == "self_collide" || + childElemA->GetName() == "enable_wind" )) + { + return testing::AssertionSuccess(); + } + } + return testing::AssertionFailure() << "No matching child element in element B for child element '" << childElemA->GetName() << "' in element A"; @@ -163,15 +221,15 @@ TEST(CompareElements, CompareWithDuplicateElements) const std::string m1Sdf = R"( - 0 0 0 0 0 0 + 1 0 0 0 0 0 )"; const std::string m1CompTestSdf = R"( - 0 0 0 0 0 0 - 0 0 0 0 0 0 + 1 0 0 0 0 0 + 0 1 0 0 0 0 )"; @@ -271,7 +329,7 @@ class ModelElementFixture : public ElementUpdateFixture auto elem = std::make_shared(); sdf::initFile("model.sdf", elem); - updateModelElement(elem, this->ecm, model); + EXPECT_TRUE(updateModelElement(elem, this->ecm, model)); return elem; } diff --git a/test/integration/save_world.cc b/test/integration/save_world.cc index 7f9f7072e2..0e3964b052 100644 --- a/test/integration/save_world.cc +++ b/test/integration/save_world.cc @@ -20,10 +20,21 @@ #include #include +#include +#include +#include #include +#include +#include +#include +#include +#include #include +#include +#include #include #include +#include #include #include @@ -428,6 +439,523 @@ TEST_F(SdfGeneratorFixture, ModelWithNestedIncludes) EXPECT_TRUE(err.empty()); } +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithSensors) +{ + this->LoadWorld("test/worlds/non_rendering_sensors.sdf"); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("non_rendering_sensors"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + EXPECT_TRUE(world->ModelNameExists("model")); + auto *model = world->ModelByName("model"); + ASSERT_NE(nullptr, model); + auto *link = model->LinkByName("link"); + ASSERT_NE(nullptr, link); + + // altimeter + { + auto *sensor = link->SensorByName("altimeter_sensor"); + ASSERT_NE(nullptr, sensor); + const sdf::Altimeter *altimeter = sensor->AltimeterSensor(); + ASSERT_NE(nullptr, altimeter); + const sdf::Noise &posNoise = altimeter->VerticalPositionNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, posNoise.Type()); + EXPECT_DOUBLE_EQ(0.1, posNoise.Mean()); + EXPECT_DOUBLE_EQ(0.2, posNoise.StdDev()); + + const sdf::Noise &velNoise = altimeter->VerticalVelocityNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, velNoise.Type()); + EXPECT_DOUBLE_EQ(2.3, velNoise.Mean()); + EXPECT_DOUBLE_EQ(4.5, velNoise.StdDev()); + } + + // contact sensor + { + // contact sensor does not have an SDF DOM class + auto *sensor = link->SensorByName("contact_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), sensor->RawPose()); + EXPECT_EQ(sdf::SensorType::CONTACT, sensor->Type()); + } + + // force torque + { + auto *sensor = link->SensorByName("force_torque_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(10, 11, 12, 0, 0, 0), sensor->RawPose()); + const sdf::ForceTorque *forceTorque = sensor->ForceTorqueSensor(); + ASSERT_NE(nullptr, forceTorque); + EXPECT_EQ(sdf::ForceTorqueFrame::CHILD, forceTorque->Frame()); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, + forceTorque->MeasureDirection()); + const sdf::Noise &forceXNoise = forceTorque->ForceXNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN_QUANTIZED, forceXNoise.Type()); + EXPECT_DOUBLE_EQ(0.02, forceXNoise.Mean()); + EXPECT_DOUBLE_EQ(0.0005, forceXNoise.StdDev()); + const sdf::Noise &torqueYNoise = forceTorque->TorqueYNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, torqueYNoise.Type()); + EXPECT_DOUBLE_EQ(0.009, torqueYNoise.Mean()); + EXPECT_DOUBLE_EQ(0.0000985, torqueYNoise.StdDev()); + } + + // imu + { + auto *sensor = link->SensorByName("imu_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), sensor->RawPose()); + const sdf::Imu *imu = sensor->ImuSensor(); + ASSERT_NE(nullptr, imu); + const sdf::Noise &linAccXNoise = imu->LinearAccelerationXNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, linAccXNoise.Type()); + EXPECT_DOUBLE_EQ(0.0, linAccXNoise.Mean()); + EXPECT_DOUBLE_EQ(0.1, linAccXNoise.StdDev()); + EXPECT_DOUBLE_EQ(0.2, linAccXNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(1.0, linAccXNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &linAccYNoise = imu->LinearAccelerationYNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, linAccYNoise.Type()); + EXPECT_DOUBLE_EQ(1.0, linAccYNoise.Mean()); + EXPECT_DOUBLE_EQ(1.1, linAccYNoise.StdDev()); + EXPECT_DOUBLE_EQ(1.2, linAccYNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(2.0, linAccYNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &linAccZNoise = imu->LinearAccelerationZNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, linAccZNoise.Type()); + EXPECT_DOUBLE_EQ(2.0, linAccZNoise.Mean()); + EXPECT_DOUBLE_EQ(2.1, linAccZNoise.StdDev()); + EXPECT_DOUBLE_EQ(2.2, linAccZNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(3.0, linAccZNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &angVelXNoise = imu->AngularVelocityXNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, angVelXNoise.Type()); + EXPECT_DOUBLE_EQ(3.0, angVelXNoise.Mean()); + EXPECT_DOUBLE_EQ(3.1, angVelXNoise.StdDev()); + EXPECT_DOUBLE_EQ(4.2, angVelXNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(4.0, angVelXNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &angVelYNoise = imu->AngularVelocityYNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, angVelYNoise.Type()); + EXPECT_DOUBLE_EQ(4.0, angVelYNoise.Mean()); + EXPECT_DOUBLE_EQ(4.1, angVelYNoise.StdDev()); + EXPECT_DOUBLE_EQ(5.2, angVelYNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(5.0, angVelYNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &angVelZNoise = imu->AngularVelocityZNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, angVelZNoise.Type()); + EXPECT_DOUBLE_EQ(5.0, angVelZNoise.Mean()); + EXPECT_DOUBLE_EQ(5.1, angVelZNoise.StdDev()); + EXPECT_DOUBLE_EQ(6.2, angVelZNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(6.0, angVelZNoise.DynamicBiasCorrelationTime()); + + EXPECT_EQ("ENU", imu->Localization()); + EXPECT_EQ("linka", imu->CustomRpyParentFrame()); + EXPECT_EQ(math::Vector3d::UnitY, imu->CustomRpy()); + EXPECT_EQ("linkb", imu->GravityDirXParentFrame()); + EXPECT_EQ(math::Vector3d::UnitZ, imu->GravityDirX()); + EXPECT_FALSE(imu->OrientationEnabled()); + } + + // logical camera + { + // logical camera sensor does not have an SDF DOM class + auto *sensor = link->SensorByName("logical_camera_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(7, 8, 9, 0, 0, 0), sensor->RawPose()); + EXPECT_EQ(sdf::SensorType::LOGICAL_CAMERA, sensor->Type()); + } + + // magnetometer + { + auto *sensor = link->SensorByName("magnetometer_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(10, 11, 12, 0, 0, 0), sensor->RawPose()); + const sdf::Magnetometer *magnetometer = sensor->MagnetometerSensor(); + ASSERT_NE(nullptr, magnetometer); + const sdf::Noise &xNoise = magnetometer->XNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, xNoise.Type()); + EXPECT_DOUBLE_EQ(0.1, xNoise.Mean()); + EXPECT_DOUBLE_EQ(0.2, xNoise.StdDev()); + const sdf::Noise &yNoise = magnetometer->YNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, yNoise.Type()); + EXPECT_DOUBLE_EQ(1.2, yNoise.Mean()); + EXPECT_DOUBLE_EQ(2.3, yNoise.StdDev()); + const sdf::Noise &zNoise = magnetometer->ZNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, zNoise.Type()); + EXPECT_DOUBLE_EQ(3.4, zNoise.Mean()); + EXPECT_DOUBLE_EQ(5.6, zNoise.StdDev()); + } + + // air pressure + { + auto *sensor = link->SensorByName("air_pressure_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(10, 20, 30, 0, 0, 0), sensor->RawPose()); + const sdf::AirPressure *airPressure = sensor->AirPressureSensor(); + ASSERT_NE(nullptr, airPressure); + EXPECT_DOUBLE_EQ(123.4, airPressure->ReferenceAltitude()); + const sdf::Noise &noise = airPressure->PressureNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, noise.Type()); + EXPECT_DOUBLE_EQ(3.4, noise.Mean()); + EXPECT_DOUBLE_EQ(5.6, noise.StdDev()); + } + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithRenderingSensors) +{ + this->LoadWorld("test/worlds/sensor.sdf"); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("camera_sensor"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + // camera + { + EXPECT_TRUE(world->ModelNameExists("camera")); + auto *model = world->ModelByName("camera"); + ASSERT_NE(nullptr, model); + EXPECT_EQ(1u, model->LinkCount()); + + auto *link = model->LinkByName("link"); + ASSERT_NE(nullptr, link); + math::MassMatrix3d massMatrix(0.1, + math::Vector3d( 0.000166667, 0.000166667, 0.000166667), + math::Vector3d::Zero); + math::Inertiald inertial(massMatrix, math::Pose3d::Zero); + EXPECT_EQ(inertial, link->Inertial()); + + auto *cameraSensor = link->SensorByName("camera"); + ASSERT_NE(nullptr, cameraSensor); + EXPECT_EQ("camera", cameraSensor->Topic()); + const sdf::Camera *camera = cameraSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.047, camera->HorizontalFov().Radian()); + EXPECT_EQ(320u, camera->ImageWidth()); + EXPECT_EQ(240u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.1, camera->NearClip()); + EXPECT_DOUBLE_EQ(100, camera->FarClip()); + const sdf::Noise &noise = camera->ImageNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN_QUANTIZED, noise.Type()); + EXPECT_DOUBLE_EQ(0.01, noise.Mean()); + EXPECT_DOUBLE_EQ(0.0002, noise.StdDev()); + } + + EXPECT_TRUE(world->ModelNameExists("default_topics")); + auto *model = world->ModelByName("default_topics"); + ASSERT_NE(nullptr, model); + // gpu lidar + { + auto *gpuLidarLink = model->LinkByName("gpu_lidar_link"); + ASSERT_NE(nullptr, gpuLidarLink); + auto *gpuLidarSensor = gpuLidarLink->SensorByName("gpu_lidar"); + const sdf::Lidar *lidar = gpuLidarSensor->LidarSensor(); + EXPECT_EQ(640u, lidar->HorizontalScanSamples()); + EXPECT_DOUBLE_EQ(1.0, lidar->HorizontalScanResolution()); + EXPECT_NEAR(-1.396263, lidar->HorizontalScanMinAngle().Radian(), 1e-5); + EXPECT_NEAR(1.396263, lidar->HorizontalScanMaxAngle().Radian(), 1e-5); + EXPECT_EQ(1u, lidar->VerticalScanSamples()); + EXPECT_DOUBLE_EQ(0.01, lidar->VerticalScanResolution()); + EXPECT_DOUBLE_EQ(0.0, lidar->VerticalScanMinAngle().Radian()); + EXPECT_DOUBLE_EQ(0.0, lidar->VerticalScanMaxAngle().Radian()); + EXPECT_DOUBLE_EQ(0.08, lidar->RangeMin()); + EXPECT_DOUBLE_EQ(10.0, lidar->RangeMax()); + EXPECT_DOUBLE_EQ(0.01, lidar->RangeResolution()); + } + + // depth camera + { + auto *depthLink = model->LinkByName("depth_camera_link"); + ASSERT_NE(nullptr, depthLink); + auto *depthSensor = depthLink->SensorByName("depth_camera"); + ASSERT_NE(nullptr, depthSensor); + const sdf::Camera *camera = depthSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.05, camera->HorizontalFov().Radian()); + EXPECT_EQ(256u, camera->ImageWidth()); + EXPECT_EQ(256u, camera->ImageHeight()); + EXPECT_EQ("R_FLOAT32", camera->PixelFormatStr()); + EXPECT_DOUBLE_EQ(0.1, camera->NearClip()); + EXPECT_DOUBLE_EQ(10, camera->FarClip()); + EXPECT_DOUBLE_EQ(0.05, camera->DepthNearClip()); + EXPECT_DOUBLE_EQ(9.0, camera->DepthFarClip()); + } + + // rgbd camera + { + auto *rgbdLink = model->LinkByName("rgbd_camera_link"); + ASSERT_NE(nullptr, rgbdLink); + auto *rgbdSensor = rgbdLink->SensorByName("rgbd_camera"); + ASSERT_NE(nullptr, rgbdSensor); + const sdf::Camera *camera = rgbdSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.05, camera->HorizontalFov().Radian()); + EXPECT_EQ(256u, camera->ImageWidth()); + EXPECT_EQ(256u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.1, camera->NearClip()); + EXPECT_DOUBLE_EQ(10, camera->FarClip()); + } + + // thermal camera + { + auto *thermalLink = model->LinkByName("thermal_camera_link"); + ASSERT_NE(nullptr, thermalLink); + auto *thermalSensor = thermalLink->SensorByName("thermal_camera"); + ASSERT_NE(nullptr, thermalSensor); + const sdf::Camera *camera = thermalSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.15, camera->HorizontalFov().Radian()); + EXPECT_EQ(300u, camera->ImageWidth()); + EXPECT_EQ(200u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.14, camera->NearClip()); + EXPECT_DOUBLE_EQ(120.0, camera->FarClip()); + } + + // segmentation camera + { + auto *segmentationLink = model->LinkByName("segmentation_camera_link"); + ASSERT_NE(nullptr, segmentationLink); + auto *segmentationSensor = + segmentationLink->SensorByName("segmentation_camera"); + ASSERT_NE(nullptr, segmentationSensor); + const sdf::Camera *camera = segmentationSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.0, camera->HorizontalFov().Radian()); + EXPECT_EQ(320u, camera->ImageWidth()); + EXPECT_EQ(240u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.11, camera->NearClip()); + EXPECT_DOUBLE_EQ(20.0, camera->FarClip()); + EXPECT_EQ("panoptic", camera->SegmentationType()); + } + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithLights) +{ + this->LoadWorld("test/worlds/lights.sdf"); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("lights"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + // directional light in the world + { + const sdf::Light *light = world->LightByIndex(0u); + EXPECT_EQ("directional", light->Name()); + EXPECT_EQ(sdf::LightType::DIRECTIONAL, light->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, 0, 10, 0, 0, 0), + light->RawPose()); + EXPECT_EQ(std::string(), light->PoseRelativeTo()); + EXPECT_TRUE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.8f, 0.8f, 0.8f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(100, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.9, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.001, light->QuadraticAttenuationFactor()); + EXPECT_EQ(ignition::math::Vector3d(0.5, 0.2, -0.9), + light->Direction()); + } + // point light in the world + { + const sdf::Light *light = world->LightByIndex(1u); + EXPECT_EQ("point", light->Name()); + EXPECT_EQ(sdf::LightType::POINT, light->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, -1.5, 3, 0, 0, 0), + light->RawPose()); + EXPECT_FALSE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(1.0f, 0.0f, 0.0f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(4, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.2, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.5, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light->QuadraticAttenuationFactor()); + } + // spot light in the world + { + const sdf::Light *light = world->LightByIndex(2u); + EXPECT_EQ("spot", light->Name()); + EXPECT_EQ(sdf::LightType::SPOT, light->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, 1.5, 3, 0, 0, 0), + light->RawPose()); + EXPECT_EQ(std::string(), light->PoseRelativeTo()); + EXPECT_FALSE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.0f, 1.0f, 0.0f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(5, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.3, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.4, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.001, light->QuadraticAttenuationFactor()); + EXPECT_EQ(ignition::math::Vector3d(0.0, 0.0, -1.0), + light->Direction()); + EXPECT_DOUBLE_EQ(0.1, light->SpotInnerAngle().Radian()); + EXPECT_DOUBLE_EQ(0.5, light->SpotOuterAngle().Radian()); + EXPECT_DOUBLE_EQ(0.8, light->SpotFalloff()); + } + + // get model + EXPECT_TRUE(world->ModelNameExists("sphere")); + auto *model = world->ModelByName("sphere"); + ASSERT_NE(nullptr, model); + EXPECT_EQ(1u, model->LinkCount()); + + // get link + auto *link = model->LinkByName("sphere_link"); + ASSERT_NE(nullptr, link); + + // light attached to link + { + const sdf::Light *light = link->LightByName("link_light_point"); + EXPECT_EQ("link_light_point", light->Name()); + EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), + light->RawPose()); + EXPECT_EQ(sdf::LightType::POINT, light->Type()); + EXPECT_FALSE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.0f, 0.0f, 1.0f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(2, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.05, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.02, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light->QuadraticAttenuationFactor()); + } + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, ModelWithJoints) +{ + this->LoadWorld(ignition::common::joinPaths("test", "worlds", + "joint_sensor.sdf")); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("joint_sensor"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ(1u, world->ModelCount()); + + EXPECT_TRUE(world->ModelNameExists("model")); + auto *model = world->ModelByName("model"); + ASSERT_NE(nullptr, model); + EXPECT_EQ(2u, model->LinkCount()); + auto *link1 = model->LinkByName("link1"); + ASSERT_NE(nullptr, link1); + auto *link2 = model->LinkByName("link2"); + ASSERT_NE(nullptr, link2); + EXPECT_EQ(1u, model->JointCount()); + auto *joint = model->JointByName("joint"); + ASSERT_NE(nullptr, joint); + + EXPECT_EQ("link1", joint->ParentLinkName()); + EXPECT_EQ("link2", joint->ChildLinkName()); + EXPECT_EQ(sdf::JointType::REVOLUTE2, joint->Type()); + + // Get the first axis + const sdf::JointAxis *axis = joint->Axis(); + ASSERT_NE(nullptr, axis); + ASSERT_NE(nullptr, axis->Element()); + + // Get the second axis + const sdf::JointAxis *axis2 = joint->Axis(1); + ASSERT_NE(nullptr, axis2); + + EXPECT_EQ(ignition::math::Vector3d::UnitZ, axis->Xyz()); + EXPECT_EQ(ignition::math::Vector3d::UnitY, axis2->Xyz()); + + EXPECT_EQ("__model__", axis->XyzExpressedIn()); + EXPECT_TRUE(axis2->XyzExpressedIn().empty()); + + EXPECT_DOUBLE_EQ(-0.5, axis->Lower()); + EXPECT_DOUBLE_EQ(0.5, axis->Upper()); + EXPECT_DOUBLE_EQ(-1.0, axis2->Lower()); + EXPECT_DOUBLE_EQ(1.0, axis2->Upper()); + + EXPECT_DOUBLE_EQ(123.4, axis->Effort()); + EXPECT_DOUBLE_EQ(0.5, axis2->Effort()); + + EXPECT_DOUBLE_EQ(12.0, axis->MaxVelocity()); + EXPECT_DOUBLE_EQ(200.0, axis2->MaxVelocity()); + + EXPECT_DOUBLE_EQ(0.1, axis->Damping()); + EXPECT_DOUBLE_EQ(0.0, axis2->Damping()); + + EXPECT_DOUBLE_EQ(0.2, axis->Friction()); + EXPECT_DOUBLE_EQ(0.0, axis2->Friction()); + + EXPECT_DOUBLE_EQ(1.3, axis->SpringReference()); + EXPECT_DOUBLE_EQ(0.0, axis2->SpringReference()); + + EXPECT_DOUBLE_EQ(10.6, axis->SpringStiffness()); + EXPECT_DOUBLE_EQ(0.0, axis2->SpringStiffness()); + + // sensor + const sdf::Sensor *forceTorqueSensor = + joint->SensorByName("force_torque_sensor"); + ASSERT_NE(nullptr, forceTorqueSensor); + EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name()); + EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + forceTorqueSensor->RawPose()); + auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor(); + ASSERT_NE(nullptr, forceTorqueSensorObj); + EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorObj->Frame()); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, + forceTorqueSensorObj->MeasureDirection()); + + EXPECT_DOUBLE_EQ(0.0, forceTorqueSensorObj->ForceXNoise().Mean()); + EXPECT_DOUBLE_EQ(0.1, forceTorqueSensorObj->ForceXNoise().StdDev()); + EXPECT_DOUBLE_EQ(1.0, forceTorqueSensorObj->ForceYNoise().Mean()); + EXPECT_DOUBLE_EQ(1.1, forceTorqueSensorObj->ForceYNoise().StdDev()); + EXPECT_DOUBLE_EQ(2.0, forceTorqueSensorObj->ForceZNoise().Mean()); + EXPECT_DOUBLE_EQ(2.1, forceTorqueSensorObj->ForceZNoise().StdDev()); + + EXPECT_DOUBLE_EQ(3.0, forceTorqueSensorObj->TorqueXNoise().Mean()); + EXPECT_DOUBLE_EQ(3.1, forceTorqueSensorObj->TorqueXNoise().StdDev()); + EXPECT_DOUBLE_EQ(4.0, forceTorqueSensorObj->TorqueYNoise().Mean()); + EXPECT_DOUBLE_EQ(4.1, forceTorqueSensorObj->TorqueYNoise().StdDev()); + EXPECT_DOUBLE_EQ(5.0, forceTorqueSensorObj->TorqueZNoise().Mean()); + EXPECT_DOUBLE_EQ(5.1, forceTorqueSensorObj->TorqueZNoise().StdDev()); +} + ///////////////////////////////////////////////// /// Main int main(int _argc, char **_argv) diff --git a/test/worlds/joint_sensor.sdf b/test/worlds/joint_sensor.sdf new file mode 100644 index 0000000000..f96cdeefe0 --- /dev/null +++ b/test/worlds/joint_sensor.sdf @@ -0,0 +1,107 @@ + + + + + + + + + + + + + link2 + link1 + + 0 0 1 + 0.5 + true + + -0.5 + 0.5 + 123.4 + 12 + + + 0.1 + 0.2 + 1.3 + 10.6 + + + + 0 1 0 + 1.5 + false + + -1 + 1 + 0.5 + 200 + + + + + 1 + + 0 + 0.2 + + + + + 10 11 12 0 0 0 + + parent + parent_to_child + + + + 0 + 0.1 + + + + + 1 + 1.1 + + + + + 2 + 2.1 + + + + + + + 3 + 3.1 + + + + + 4 + 4.1 + + + + + 5 + 5.1 + + + + + + + + + + diff --git a/test/worlds/non_rendering_sensors.sdf b/test/worlds/non_rendering_sensors.sdf new file mode 100644 index 0000000000..f5fac8c8aa --- /dev/null +++ b/test/worlds/non_rendering_sensors.sdf @@ -0,0 +1,176 @@ + + + + + ogre2 + + + + + + + 0 0 3 0 0 0 + + + + + 0.1 + 0.2 + + + + + 2.3 + 4.5 + + + + + + + 4 5 6 0 0 0 + true + + + + 10 11 12 0 0 0 + + child + parent_to_child + + + + 0.02 + 0.0005 + + + + + + + 0.009 + 0.0000985 + + + + + + + + 4 5 6 0 0 0 + + + + + 0 + 0.1 + 0.2 + 1 + + + + + 1 + 1.1 + 1.2 + 2 + + + + + 2 + 2.1 + 2.2 + 3 + + + + + + + 3 + 3.1 + 4.2 + 4 + + + + + 4 + 4.1 + 5.2 + 5 + + + + + 5 + 5.1 + 6.2 + 6 + + + + + ENU + 0 1 0 + 0 0 1 + + false + + + + + 7 8 9 0 0 0 + + 0.1 + 100.1 + 1.33 + 1.234 + + + + + 10 11 12 0 0 0 + + + + 0.1 + 0.2 + + + + + 1.2 + 2.3 + + + + + 3.4 + 5.6 + + + + + + + 10 20 30 0 0 0 + + 123.4 + + + 3.4 + 5.6 + + + + + + + + diff --git a/test/worlds/save_world.sdf b/test/worlds/save_world.sdf index e6a431d4e6..226ef45cb5 100644 --- a/test/worlds/save_world.sdf +++ b/test/worlds/save_world.sdf @@ -13,6 +13,21 @@ filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands"> + 0 0 -9.8 + 5.5645e-6 22.8758e-6 -42.3884e-6 + + + + 0.4 0.4 0.4 1.0 + .7 .7 .7 1 + true + + + + 0.001 + 1.0 + 1000 + 10 0 0 0 0 0 diff --git a/test/worlds/sensor.sdf b/test/worlds/sensor.sdf index aebb32104c..1587a32736 100644 --- a/test/worlds/sensor.sdf +++ b/test/worlds/sensor.sdf @@ -64,6 +64,11 @@ 0.1 100 + + gaussian_quantized + 0.01 + 0.0002 + 1 30 @@ -131,6 +136,12 @@ 0.1 10.0 + + + 0.05 + 9.0 + + @@ -150,6 +161,40 @@ + + + + + 1.15 + + 300 + 200 + + + 0.14 + 120.0 + + + + + + + + + 1.0 + + 320 + 240 + + + 0.11 + 20.0 + + panoptic + + + +