diff --git a/src/Geometry.cc b/src/Geometry.cc index b7245bcd9..ba11a6382 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -14,6 +14,9 @@ * limitations under the License. * */ + +#include + #include "sdf/Geometry.hh" #include "sdf/Box.hh" #include "sdf/Capsule.hh" @@ -24,6 +27,8 @@ #include "sdf/Plane.hh" #include "sdf/Sphere.hh" +#include "Utils.hh" + using namespace sdf; // Private data class @@ -32,29 +37,29 @@ class sdf::GeometryPrivate // \brief The geometry type. public: GeometryType type = GeometryType::EMPTY; - /// \brief Pointer to a box. - public: std::unique_ptr box; + /// \brief Optional box. + public: std::optional box; - /// \brief Pointer to a capsule. - public: std::unique_ptr capsule; + /// \brief Optional capsule. + public: std::optional capsule; - /// \brief Pointer to a cylinder. - public: std::unique_ptr cylinder; + /// \brief Optional cylinder. + public: std::optional cylinder; - /// \brief Pointer to an ellipsoid - public: std::unique_ptr ellipsoid; + /// \brief Optional ellipsoid + public: std::optional ellipsoid; - /// \brief Pointer to a plane. - public: std::unique_ptr plane; + /// \brief Optional plane. + public: std::optional plane; - /// \brief Pointer to a sphere. - public: std::unique_ptr sphere; + /// \brief Optional sphere. + public: std::optional sphere; - /// \brief Pointer to a mesh. - public: std::unique_ptr mesh; + /// \brief Optional mesh. + public: std::optional mesh; - /// \brief Pointer to a heightmap. - public: std::unique_ptr heightmap; + /// \brief Optional heightmap. + public: std::optional heightmap; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -75,57 +80,8 @@ Geometry::~Geometry() ////////////////////////////////////////////////// Geometry::Geometry(const Geometry &_geometry) - : dataPtr(new GeometryPrivate) + : dataPtr(new GeometryPrivate(*_geometry.dataPtr)) { - this->dataPtr->type = _geometry.dataPtr->type; - - if (_geometry.dataPtr->box) - { - this->dataPtr->box = std::make_unique(*_geometry.dataPtr->box); - } - - if (_geometry.dataPtr->capsule) - { - this->dataPtr->capsule = std::make_unique( - *_geometry.dataPtr->capsule); - } - - if (_geometry.dataPtr->cylinder) - { - this->dataPtr->cylinder = std::make_unique( - *_geometry.dataPtr->cylinder); - } - - if (_geometry.dataPtr->ellipsoid) - { - this->dataPtr->ellipsoid = std::make_unique( - *_geometry.dataPtr->ellipsoid); - } - - if (_geometry.dataPtr->plane) - { - this->dataPtr->plane = std::make_unique( - *_geometry.dataPtr->plane); - } - - if (_geometry.dataPtr->sphere) - { - this->dataPtr->sphere = std::make_unique( - *_geometry.dataPtr->sphere); - } - - if (_geometry.dataPtr->mesh) - { - this->dataPtr->mesh = std::make_unique(*_geometry.dataPtr->mesh); - } - - if (_geometry.dataPtr->heightmap) - { - this->dataPtr->heightmap = - std::make_unique(*_geometry.dataPtr->heightmap); - } - - this->dataPtr->sdf = _geometry.dataPtr->sdf; } ////////////////////////////////////////////////// @@ -176,56 +132,56 @@ Errors Geometry::Load(ElementPtr _sdf) if (_sdf->HasElement("box")) { this->dataPtr->type = GeometryType::BOX; - this->dataPtr->box.reset(new Box()); + this->dataPtr->box.emplace(); Errors err = this->dataPtr->box->Load(_sdf->GetElement("box")); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("capsule")) { this->dataPtr->type = GeometryType::CAPSULE; - this->dataPtr->capsule.reset(new Capsule()); + this->dataPtr->capsule.emplace(); Errors err = this->dataPtr->capsule->Load(_sdf->GetElement("capsule")); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("cylinder")) { this->dataPtr->type = GeometryType::CYLINDER; - this->dataPtr->cylinder.reset(new Cylinder()); + this->dataPtr->cylinder.emplace(); Errors err = this->dataPtr->cylinder->Load(_sdf->GetElement("cylinder")); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("ellipsoid")) { this->dataPtr->type = GeometryType::ELLIPSOID; - this->dataPtr->ellipsoid.reset(new Ellipsoid()); + this->dataPtr->ellipsoid.emplace(); Errors err = this->dataPtr->ellipsoid->Load(_sdf->GetElement("ellipsoid")); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("plane")) { this->dataPtr->type = GeometryType::PLANE; - this->dataPtr->plane.reset(new Plane()); + this->dataPtr->plane.emplace(); Errors err = this->dataPtr->plane->Load(_sdf->GetElement("plane")); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("sphere")) { this->dataPtr->type = GeometryType::SPHERE; - this->dataPtr->sphere.reset(new Sphere()); + this->dataPtr->sphere.emplace(); Errors err = this->dataPtr->sphere->Load(_sdf->GetElement("sphere")); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("mesh")) { this->dataPtr->type = GeometryType::MESH; - this->dataPtr->mesh.reset(new Mesh()); + this->dataPtr->mesh.emplace(); Errors err = this->dataPtr->mesh->Load(_sdf->GetElement("mesh")); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("heightmap")) { this->dataPtr->type = GeometryType::HEIGHTMAP; - this->dataPtr->heightmap.reset(new Heightmap()); + this->dataPtr->heightmap.emplace(); Errors err = this->dataPtr->heightmap->Load(_sdf->GetElement("heightmap")); errors.insert(errors.end(), err.begin(), err.end()); } @@ -248,97 +204,97 @@ void Geometry::SetType(const GeometryType _type) ///////////////////////////////////////////////// const Box *Geometry::BoxShape() const { - return this->dataPtr->box.get(); + return optionalToPointer(this->dataPtr->box); } ///////////////////////////////////////////////// void Geometry::SetBoxShape(const Box &_box) { - this->dataPtr->box = std::make_unique(_box); + this->dataPtr->box = _box; } ///////////////////////////////////////////////// const Sphere *Geometry::SphereShape() const { - return this->dataPtr->sphere.get(); + return optionalToPointer(this->dataPtr->sphere); } ///////////////////////////////////////////////// void Geometry::SetSphereShape(const Sphere &_sphere) { - this->dataPtr->sphere = std::make_unique(_sphere); + this->dataPtr->sphere = _sphere; } ///////////////////////////////////////////////// const Capsule *Geometry::CapsuleShape() const { - return this->dataPtr->capsule.get(); + return optionalToPointer(this->dataPtr->capsule); } ///////////////////////////////////////////////// void Geometry::SetCapsuleShape(const Capsule &_capsule) { - this->dataPtr->capsule = std::make_unique(_capsule); + this->dataPtr->capsule = _capsule; } ///////////////////////////////////////////////// const Cylinder *Geometry::CylinderShape() const { - return this->dataPtr->cylinder.get(); + return optionalToPointer(this->dataPtr->cylinder); } ///////////////////////////////////////////////// void Geometry::SetCylinderShape(const Cylinder &_cylinder) { - this->dataPtr->cylinder = std::make_unique(_cylinder); + this->dataPtr->cylinder = _cylinder; } ///////////////////////////////////////////////// const Ellipsoid *Geometry::EllipsoidShape() const { - return this->dataPtr->ellipsoid.get(); + return optionalToPointer(this->dataPtr->ellipsoid); } ///////////////////////////////////////////////// void Geometry::SetEllipsoidShape(const Ellipsoid &_ellipsoid) { - this->dataPtr->ellipsoid = std::make_unique(_ellipsoid); + this->dataPtr->ellipsoid = _ellipsoid; } ///////////////////////////////////////////////// const Plane *Geometry::PlaneShape() const { - return this->dataPtr->plane.get(); + return optionalToPointer(this->dataPtr->plane); } ///////////////////////////////////////////////// void Geometry::SetPlaneShape(const Plane &_plane) { - this->dataPtr->plane = std::make_unique(_plane); + this->dataPtr->plane = _plane; } ///////////////////////////////////////////////// const Mesh *Geometry::MeshShape() const { - return this->dataPtr->mesh.get(); + return optionalToPointer(this->dataPtr->mesh); } ///////////////////////////////////////////////// void Geometry::SetMeshShape(const Mesh &_mesh) { - this->dataPtr->mesh = std::make_unique(_mesh); + this->dataPtr->mesh = _mesh; } ///////////////////////////////////////////////// const Heightmap *Geometry::HeightmapShape() const { - return this->dataPtr->heightmap.get(); + return optionalToPointer(this->dataPtr->heightmap); } ///////////////////////////////////////////////// void Geometry::SetHeightmapShape(const Heightmap &_heightmap) { - this->dataPtr->heightmap = std::make_unique(_heightmap); + this->dataPtr->heightmap = _heightmap; } ///////////////////////////////////////////////// diff --git a/src/Joint.cc b/src/Joint.cc index 711dffb9d..5e67aeade 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -32,21 +32,6 @@ using namespace sdf; class sdf::JointPrivate { - public: JointPrivate() - { - // Initialize here because windows does not support list initialization - // at member initialization (ie ... axis = {{nullptr, nullpter}};). - this->axis[0] = nullptr; - this->axis[1] = nullptr; - } - - /// \brief Copy constructor - /// \param[in] _jointPrivate JointPrivate to copy. - public: explicit JointPrivate(const JointPrivate &_jointPrivate); - - // Delete copy assignment so it is not accidentally used - public: JointPrivate &operator=(const JointPrivate &_) = delete; - /// \brief Name of the joint. public: std::string name = ""; @@ -70,7 +55,7 @@ class sdf::JointPrivate /// \brief Joint axis // cppcheck-suppress - public: std::array, 2> axis; + public: std::array, 2> axis; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -82,28 +67,6 @@ class sdf::JointPrivate public: sdf::ScopedGraph poseRelativeToGraph; }; -///////////////////////////////////////////////// -JointPrivate::JointPrivate(const JointPrivate &_jointPrivate) - : name(_jointPrivate.name), - parentLinkName(_jointPrivate.parentLinkName), - childLinkName(_jointPrivate.childLinkName), - type(_jointPrivate.type), - pose(_jointPrivate.pose), - poseRelativeTo(_jointPrivate.poseRelativeTo), - threadPitch(_jointPrivate.threadPitch), - sdf(_jointPrivate.sdf), - frameAttachedToGraph(_jointPrivate.frameAttachedToGraph), - poseRelativeToGraph(_jointPrivate.poseRelativeToGraph) -{ - for (std::size_t i = 0; i < _jointPrivate.axis.size(); ++i) - { - if (_jointPrivate.axis[i]) - { - this->axis[i] = std::make_unique(*_jointPrivate.axis[i]); - } - } -} - ///////////////////////////////////////////////// Joint::Joint() : dataPtr(new JointPrivate) @@ -232,14 +195,14 @@ Errors Joint::Load(ElementPtr _sdf) if (_sdf->HasElement("axis")) { - this->dataPtr->axis[0].reset(new JointAxis()); + this->dataPtr->axis[0].emplace(); Errors axisErrors = this->dataPtr->axis[0]->Load(_sdf->GetElement("axis")); errors.insert(errors.end(), axisErrors.begin(), axisErrors.end()); } if (_sdf->HasElement("axis2")) { - this->dataPtr->axis[1].reset(new JointAxis()); + this->dataPtr->axis[1].emplace(); Errors axisErrors = this->dataPtr->axis[1]->Load(_sdf->GetElement("axis2")); errors.insert(errors.end(), axisErrors.begin(), axisErrors.end()); } @@ -338,14 +301,13 @@ void Joint::SetChildLinkName(const std::string &_name) ///////////////////////////////////////////////// const JointAxis *Joint::Axis(const unsigned int _index) const { - return this->dataPtr->axis[std::min(_index, 1u)].get(); + return optionalToPointer(this->dataPtr->axis[std::min(_index, 1u)]); } ///////////////////////////////////////////////// void Joint::SetAxis(const unsigned int _index, const JointAxis &_axis) { - this->dataPtr->axis[std::min(_index, 1u)] = - std::make_unique(_axis); + this->dataPtr->axis[std::min(_index, 1u)] = _axis; } ///////////////////////////////////////////////// diff --git a/src/Material.cc b/src/Material.cc index 41bf0a0f2..ed93a472b 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -15,6 +15,7 @@ * */ #include +#include #include #include @@ -61,7 +62,7 @@ class sdf::MaterialPrivate public: float renderOrder = 0; /// \brief Physically Based Rendering (PBR) properties - public: std::unique_ptr pbr; + public: std::optional pbr; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -85,23 +86,8 @@ Material::~Material() ////////////////////////////////////////////////// Material::Material(const Material &_material) - : dataPtr(new MaterialPrivate) + : dataPtr(new MaterialPrivate(*_material.dataPtr)) { - this->dataPtr->scriptUri = _material.dataPtr->scriptUri; - this->dataPtr->scriptName = _material.dataPtr->scriptName; - this->dataPtr->shader = _material.dataPtr->shader; - this->dataPtr->normalMap = _material.dataPtr->normalMap; - this->dataPtr->lighting = _material.dataPtr->lighting; - this->dataPtr->renderOrder = _material.dataPtr->renderOrder; - this->dataPtr->doubleSided = _material.dataPtr->doubleSided; - this->dataPtr->ambient = _material.dataPtr->ambient; - this->dataPtr->diffuse = _material.dataPtr->diffuse; - this->dataPtr->specular = _material.dataPtr->specular; - this->dataPtr->emissive = _material.dataPtr->emissive; - this->dataPtr->sdf = _material.dataPtr->sdf; - this->dataPtr->filePath = _material.dataPtr->filePath; - if (_material.dataPtr->pbr) - this->dataPtr->pbr = std::make_unique(*_material.dataPtr->pbr); } ///////////////////////////////////////////////// @@ -235,7 +221,7 @@ Errors Material::Load(sdf::ElementPtr _sdf) // load pbr param if (_sdf->HasElement("pbr")) { - this->dataPtr->pbr.reset(new sdf::Pbr()); + this->dataPtr->pbr.emplace(); Errors pbrErrors = this->dataPtr->pbr->Load(_sdf->GetElement("pbr")); errors.insert(errors.end(), pbrErrors.begin(), pbrErrors.end()); } @@ -384,13 +370,13 @@ void Material::SetNormalMap(const std::string &_map) ////////////////////////////////////////////////// void Material::SetPbrMaterial(const Pbr &_pbr) { - this->dataPtr->pbr.reset(new Pbr(_pbr)); + this->dataPtr->pbr = _pbr; } ////////////////////////////////////////////////// Pbr *Material::PbrMaterial() const { - return this->dataPtr->pbr.get(); + return optionalToPointer(this->dataPtr->pbr); } ////////////////////////////////////////////////// diff --git a/src/Scene.cc b/src/Scene.cc index a042a1b07..b7ffd6bd3 100644 --- a/src/Scene.cc +++ b/src/Scene.cc @@ -22,16 +22,6 @@ using namespace sdf; /// \brief Scene private data. class sdf::ScenePrivate { - /// \brief Default constructor - public: ScenePrivate() = default; - - /// \brief Copy constructor - /// \param[in] _scenePrivate private data to copy - public: explicit ScenePrivate(const ScenePrivate &_scenePrivate); - - // Delete copy assignment so it is not accidentally used - public: ScenePrivate &operator=(const ScenePrivate &) = delete; - /// \brief True if grid should be enabled public: bool grid = true; @@ -50,28 +40,12 @@ class sdf::ScenePrivate ignition::math::Color(0.7f, 0.7f, .7f); /// \brief Pointer to the sky properties. - public: std::unique_ptr sky; + public: std::optional sky; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; }; -///////////////////////////////////////////////// -ScenePrivate::ScenePrivate(const ScenePrivate &_scenePrivate) - : grid(_scenePrivate.grid), - shadows(_scenePrivate.shadows), - originVisual(_scenePrivate.originVisual), - ambient(_scenePrivate.ambient), - background(_scenePrivate.background), - sdf(_scenePrivate.sdf) -{ - if (_scenePrivate.sky) - { - this->sky = - std::make_unique(*(_scenePrivate.sky)); - } -} - ///////////////////////////////////////////////// Scene::Scene() : dataPtr(new ScenePrivate) @@ -150,7 +124,7 @@ Errors Scene::Load(ElementPtr _sdf) // load sky if (_sdf->HasElement("sky")) { - this->dataPtr->sky = std::make_unique(); + this->dataPtr->sky.emplace(); Errors err = this->dataPtr->sky->Load(_sdf->GetElement("sky")); errors.insert(errors.end(), err.begin(), err.end()); } @@ -220,13 +194,13 @@ void Scene::SetOriginVisual(const bool _enabled) ///////////////////////////////////////////////// void Scene::SetSky(const sdf::Sky &_sky) { - this->dataPtr->sky = std::make_unique(_sky); + this->dataPtr->sky = _sky; } ///////////////////////////////////////////////// const sdf::Sky *Scene::Sky() const { - return this->dataPtr->sky.get(); + return optionalToPointer(this->dataPtr->sky); } ///////////////////////////////////////////////// diff --git a/src/Sensor.cc b/src/Sensor.cc index 842ceae32..92f9bb1c6 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include #include #include "sdf/AirPressure.hh" @@ -63,58 +64,6 @@ const std::vector sensorTypeStrs = class sdf::SensorPrivate { - /// \brief Default constructor - public: SensorPrivate() = default; - - /// \brief Copy constructor - public: explicit SensorPrivate(const SensorPrivate &_sensor) - : type(_sensor.type), - name(_sensor.name), - topic(_sensor.topic), - pose(_sensor.pose), - poseRelativeTo(_sensor.poseRelativeTo), - sdf(_sensor.sdf), - updateRate(_sensor.updateRate) - - { - if (_sensor.magnetometer) - { - this->magnetometer = std::make_unique( - *_sensor.magnetometer); - } - if (_sensor.altimeter) - { - this->altimeter = std::make_unique(*_sensor.altimeter); - } - if (_sensor.airPressure) - { - this->airPressure = std::make_unique( - *_sensor.airPressure); - } - if (_sensor.camera) - { - this->camera = std::make_unique(*_sensor.camera); - } - if (_sensor.forceTorque) - { - this->forceTorque = std::make_unique( - *_sensor.forceTorque); - } - if (_sensor.imu) - { - this->imu = std::make_unique(*_sensor.imu); - } - if (_sensor.lidar) - { - this->lidar = std::make_unique(*_sensor.lidar); - } - // Developer note: If you add a new sensor type, make sure to also - // update the Sensor::operator== function. Please bump this text down as - // new sensors are added so that the next developer sees the message. - } - // Delete copy assignment so it is not accidentally used - public: SensorPrivate &operator=(const SensorPrivate &) = delete; - // \brief The sensor type. public: SensorType type = SensorType::NONE; @@ -139,26 +88,26 @@ class sdf::SensorPrivate /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; - /// \brief Pointer to a magnetometer. - public: std::unique_ptr magnetometer; + /// \brief Optional magnetometer. + public: std::optional magnetometer; - /// \brief Pointer to an altimeter. - public: std::unique_ptr altimeter; + /// \brief Optional altimeter. + public: std::optional altimeter; - /// \brief Pointer to an air pressure sensor. - public: std::unique_ptr airPressure; + /// \brief Optional air pressure sensor. + public: std::optional airPressure; - /// \brief Pointer to a camera. - public: std::unique_ptr camera; + /// \brief Optional camera. + public: std::optional camera; - /// \brief Pointer to a force torque sensor. - public: std::unique_ptr forceTorque; + /// \brief Optional force torque sensor. + public: std::optional forceTorque; - /// \brief Pointer to an IMU. - public: std::unique_ptr imu; + /// \brief Optional IMU. + public: std::optional imu; - /// \brief Pointer to a lidar. - public: std::unique_ptr lidar; + /// \brief Optional lidar. + public: std::optional lidar; // Developer note: If you add a new sensor type, make sure to also // update the Sensor::operator== function. Please bump this text down as @@ -305,7 +254,7 @@ Errors Sensor::Load(ElementPtr _sdf) if (type == "air_pressure") { this->dataPtr->type = SensorType::AIR_PRESSURE; - this->dataPtr->airPressure.reset(new AirPressure()); + this->dataPtr->airPressure.emplace(); Errors err = this->dataPtr->airPressure->Load( _sdf->GetElement("air_pressure")); errors.insert(errors.end(), err.begin(), err.end()); @@ -313,14 +262,14 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "altimeter") { this->dataPtr->type = SensorType::ALTIMETER; - this->dataPtr->altimeter.reset(new Altimeter()); + this->dataPtr->altimeter.emplace(); Errors err = this->dataPtr->altimeter->Load(_sdf->GetElement("altimeter")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "camera") { this->dataPtr->type = SensorType::CAMERA; - this->dataPtr->camera.reset(new Camera()); + this->dataPtr->camera.emplace(); Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); errors.insert(errors.end(), err.begin(), err.end()); } @@ -331,28 +280,28 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "depth" || type == "depth_camera") { this->dataPtr->type = SensorType::DEPTH_CAMERA; - this->dataPtr->camera.reset(new Camera()); + this->dataPtr->camera.emplace(); Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "rgbd" || type == "rgbd_camera") { this->dataPtr->type = SensorType::RGBD_CAMERA; - this->dataPtr->camera.reset(new Camera()); + this->dataPtr->camera.emplace(); Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "thermal" || type == "thermal_camera") { this->dataPtr->type = SensorType::THERMAL_CAMERA; - this->dataPtr->camera.reset(new Camera()); + this->dataPtr->camera.emplace(); Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "force_torque") { this->dataPtr->type = SensorType::FORCE_TORQUE; - this->dataPtr->forceTorque.reset(new ForceTorque()); + this->dataPtr->forceTorque.emplace(); Errors err = this->dataPtr->forceTorque->Load( _sdf->GetElement("force_torque")); errors.insert(errors.end(), err.begin(), err.end()); @@ -364,7 +313,7 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "gpu_ray" || type == "gpu_lidar") { this->dataPtr->type = SensorType::GPU_LIDAR; - this->dataPtr->lidar.reset(new Lidar()); + this->dataPtr->lidar.emplace(); Errors err = this->dataPtr->lidar->Load( _sdf->GetElement(_sdf->HasElement("lidar") ? "lidar" : "ray")); errors.insert(errors.end(), err.begin(), err.end()); @@ -372,7 +321,7 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "imu") { this->dataPtr->type = SensorType::IMU; - this->dataPtr->imu.reset(new Imu()); + this->dataPtr->imu.emplace(); Errors err = this->dataPtr->imu->Load(_sdf->GetElement("imu")); errors.insert(errors.end(), err.begin(), err.end()); } @@ -383,7 +332,7 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "magnetometer") { this->dataPtr->type = SensorType::MAGNETOMETER; - this->dataPtr->magnetometer.reset(new Magnetometer()); + this->dataPtr->magnetometer.emplace(); Errors err = this->dataPtr->magnetometer->Load( _sdf->GetElement("magnetometer")); errors.insert(errors.end(), err.begin(), err.end()); @@ -395,7 +344,7 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "ray" || type == "lidar") { this->dataPtr->type = SensorType::LIDAR; - this->dataPtr->lidar.reset(new Lidar()); + this->dataPtr->lidar.emplace(); Errors err = this->dataPtr->lidar->Load( _sdf->GetElement(_sdf->HasElement("lidar") ? "lidar" : "ray")); errors.insert(errors.end(), err.begin(), err.end()); @@ -540,49 +489,49 @@ bool Sensor::SetType(const std::string &_typeStr) ///////////////////////////////////////////////// const Magnetometer *Sensor::MagnetometerSensor() const { - return this->dataPtr->magnetometer.get(); + return optionalToPointer(this->dataPtr->magnetometer); } ///////////////////////////////////////////////// void Sensor::SetMagnetometerSensor(const Magnetometer &_mag) { - this->dataPtr->magnetometer = std::make_unique(_mag); + this->dataPtr->magnetometer = _mag; } ///////////////////////////////////////////////// const Altimeter *Sensor::AltimeterSensor() const { - return this->dataPtr->altimeter.get(); + return optionalToPointer(this->dataPtr->altimeter); } ///////////////////////////////////////////////// void Sensor::SetAltimeterSensor(const Altimeter &_alt) { - this->dataPtr->altimeter = std::make_unique(_alt); + this->dataPtr->altimeter = _alt; } ///////////////////////////////////////////////// const AirPressure *Sensor::AirPressureSensor() const { - return this->dataPtr->airPressure.get(); + return optionalToPointer(this->dataPtr->airPressure); } ///////////////////////////////////////////////// void Sensor::SetAirPressureSensor(const AirPressure &_air) { - this->dataPtr->airPressure = std::make_unique(_air); + this->dataPtr->airPressure = _air; } ///////////////////////////////////////////////// const Lidar *Sensor::LidarSensor() const { - return this->dataPtr->lidar.get(); + return optionalToPointer(this->dataPtr->lidar); } ///////////////////////////////////////////////// void Sensor::SetLidarSensor(const Lidar &_lidar) { - this->dataPtr->lidar = std::make_unique(_lidar); + this->dataPtr->lidar = _lidar; } ///////////////////////////////////////////////// @@ -609,35 +558,35 @@ std::string Sensor::TypeStr() const ///////////////////////////////////////////////// void Sensor::SetCameraSensor(const Camera &_cam) { - this->dataPtr->camera = std::make_unique(_cam); + this->dataPtr->camera = _cam; } ///////////////////////////////////////////////// const Camera *Sensor::CameraSensor() const { - return this->dataPtr->camera.get(); + return optionalToPointer(this->dataPtr->camera); } ///////////////////////////////////////////////// void Sensor::SetForceTorqueSensor(const ForceTorque &_ft) { - this->dataPtr->forceTorque = std::make_unique(_ft); + this->dataPtr->forceTorque = _ft; } ///////////////////////////////////////////////// const ForceTorque *Sensor::ForceTorqueSensor() const { - return this->dataPtr->forceTorque.get(); + return optionalToPointer(this->dataPtr->forceTorque); } ///////////////////////////////////////////////// void Sensor::SetImuSensor(const Imu &_imu) { - this->dataPtr->imu = std::make_unique(_imu); + this->dataPtr->imu = _imu; } ///////////////////////////////////////////////// const Imu *Sensor::ImuSensor() const { - return this->dataPtr->imu.get(); + return optionalToPointer(this->dataPtr->imu); } diff --git a/src/Utils.hh b/src/Utils.hh index c49489b26..ae9d945ee 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include "sdf/Error.hh" #include "sdf/Element.hh" @@ -181,6 +182,29 @@ namespace sdf return errors; } + + /// \brief Convenience function that returns a pointer to the value contained + /// in a std::optional. + /// \tparam T type of object contained in the std::optional + /// \param[in] _opt Input optional object. + /// \return A pointer to the value contained in the optional. A nullptr is + /// returned if the optional does not contain a value. + template + T *optionalToPointer(std::optional &_opt) + { + if (_opt) + return &_opt.value(); + return nullptr; + } + + /// \brief const overload of optionalToPointer(std::optional &_opt) + template + const T *optionalToPointer(const std::optional &_opt) + { + if (_opt) + return &_opt.value(); + return nullptr; + } } } #endif diff --git a/src/Visual.cc b/src/Visual.cc index ac7c4ae47..4a830b865 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -29,16 +29,6 @@ using namespace sdf; class sdf::VisualPrivate { - /// \brief Default constructor - public: VisualPrivate() = default; - - /// \brief Copy constructor - /// \param[in] _visualPrivate Joint axis to move. - public: explicit VisualPrivate(const VisualPrivate &_visualPrivate); - - // Delete copy assignment so it is not accidentally used - public: VisualPrivate &operator=(const VisualPrivate &) = delete; - /// \brief Name of the visual. public: std::string name = ""; @@ -60,8 +50,8 @@ class sdf::VisualPrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Pointer to the visual's material properties. - public: std::unique_ptr material; + /// \brief The visual's material properties. + public: std::optional material; /// \brief Name of xml parent object. public: std::string xmlParentName; @@ -73,23 +63,6 @@ class sdf::VisualPrivate public: uint32_t visibilityFlags = 4294967295u; }; -///////////////////////////////////////////////// -VisualPrivate::VisualPrivate(const VisualPrivate &_visualPrivate) - : name(_visualPrivate.name), - castShadows(_visualPrivate.castShadows), - transparency(_visualPrivate.transparency), - pose(_visualPrivate.pose), - poseRelativeTo(_visualPrivate.poseRelativeTo), - geom(_visualPrivate.geom), - sdf(_visualPrivate.sdf), - visibilityFlags(_visualPrivate.visibilityFlags) -{ - if (_visualPrivate.material) - { - this->material = std::make_unique(*(_visualPrivate.material)); - } -} - ///////////////////////////////////////////////// Visual::Visual() : dataPtr(new VisualPrivate) @@ -175,7 +148,7 @@ Errors Visual::Load(ElementPtr _sdf) if (_sdf->HasElement("material")) { - this->dataPtr->material.reset(new sdf::Material()); + this->dataPtr->material.emplace(); Errors err = this->dataPtr->material->Load(_sdf->GetElement("material")); errors.insert(errors.end(), err.begin(), err.end()); } @@ -302,13 +275,13 @@ sdf::ElementPtr Visual::Element() const ///////////////////////////////////////////////// sdf::Material *Visual::Material() const { - return this->dataPtr->material.get(); + return optionalToPointer(this->dataPtr->material); } ///////////////////////////////////////////////// void Visual::SetMaterial(const sdf::Material &_material) { - this->dataPtr->material.reset(new sdf::Material(_material)); + this->dataPtr->material = _material; } ///////////////////////////////////////////////// diff --git a/src/World.cc b/src/World.cc index 704408e13..7223394a7 100644 --- a/src/World.cc +++ b/src/World.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "sdf/Actor.hh" @@ -34,18 +35,8 @@ using namespace sdf; class sdf::WorldPrivate { - /// \brief Default constructor - public: WorldPrivate() = default; - - /// \brief Copy constructor - /// \param[in] _worldPrivate Joint axis to move. - public: explicit WorldPrivate(const WorldPrivate &_worldPrivate); - - // Delete copy assignment so it is not accidentally used - public: WorldPrivate &operator=(const WorldPrivate &) = delete; - - /// \brief Pointer to an atmosphere model. - public: std::unique_ptr atmosphere; + /// \brief Optional atmosphere model. + public: std::optional atmosphere; /// \brief Audio device name public: std::string audioDevice = "default"; @@ -54,11 +45,11 @@ class sdf::WorldPrivate public: ignition::math::Vector3d gravity = ignition::math::Vector3d(0, 0, -9.80665); - /// \brief Pointer to Gui parameters. - public: std::unique_ptr gui; + /// \brief Optional Gui parameters. + public: std::optional gui; - /// \brief Pointer to Sene parameters. - public: std::unique_ptr scene; + /// \brief Optional Scene parameters. + public: std::optional scene; /// \brief The frames specified in this world. public: std::vector frames; @@ -98,36 +89,6 @@ class sdf::WorldPrivate public: sdf::ScopedGraph poseRelativeToGraph; }; -///////////////////////////////////////////////// -WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) - : audioDevice(_worldPrivate.audioDevice), - gravity(_worldPrivate.gravity), - frames(_worldPrivate.frames), - lights(_worldPrivate.lights), - actors(_worldPrivate.actors), - magneticField(_worldPrivate.magneticField), - models(_worldPrivate.models), - name(_worldPrivate.name), - physics(_worldPrivate.physics), - sdf(_worldPrivate.sdf), - windLinearVelocity(_worldPrivate.windLinearVelocity), - frameAttachedToGraph(_worldPrivate.frameAttachedToGraph), - poseRelativeToGraph(_worldPrivate.poseRelativeToGraph) -{ - if (_worldPrivate.atmosphere) - { - this->atmosphere = - std::make_unique(*(_worldPrivate.atmosphere)); - } - if (_worldPrivate.gui) - { - this->gui = std::make_unique(*(_worldPrivate.gui)); - } - if (_worldPrivate.scene) - { - this->scene = std::make_unique(*(_worldPrivate.scene)); - } -} ///////////////////////////////////////////////// World::World() @@ -220,7 +181,7 @@ Errors World::Load(sdf::ElementPtr _sdf) // Read the atmosphere element if (_sdf->HasElement("atmosphere")) { - this->dataPtr->atmosphere.reset(new sdf::Atmosphere()); + this->dataPtr->atmosphere.emplace(); Errors atmosphereLoadErrors = this->dataPtr->atmosphere->Load(_sdf->GetElement("atmosphere")); errors.insert(errors.end(), atmosphereLoadErrors.begin(), @@ -308,7 +269,7 @@ Errors World::Load(sdf::ElementPtr _sdf) // Load the Gui if (_sdf->HasElement("gui")) { - this->dataPtr->gui.reset(new sdf::Gui()); + this->dataPtr->gui.emplace(); Errors guiLoadErrors = this->dataPtr->gui->Load(_sdf->GetElement("gui")); errors.insert(errors.end(), guiLoadErrors.begin(), guiLoadErrors.end()); } @@ -316,7 +277,7 @@ Errors World::Load(sdf::ElementPtr _sdf) // Load the Scene if (_sdf->HasElement("scene")) { - this->dataPtr->scene.reset(new sdf::Scene()); + this->dataPtr->scene.emplace(); Errors sceneLoadErrors = this->dataPtr->scene->Load(_sdf->GetElement("scene")); errors.insert(errors.end(), sceneLoadErrors.begin(), sceneLoadErrors.end()); @@ -428,37 +389,37 @@ const Model *World::ModelByName(const std::string &_name) const ///////////////////////////////////////////////// const sdf::Atmosphere *World::Atmosphere() const { - return this->dataPtr->atmosphere.get(); + return optionalToPointer(this->dataPtr->atmosphere); } ///////////////////////////////////////////////// void World::SetAtmosphere(const sdf::Atmosphere &_atmosphere) const { - this->dataPtr->atmosphere.reset(new sdf::Atmosphere(_atmosphere)); + this->dataPtr->atmosphere = _atmosphere; } ///////////////////////////////////////////////// sdf::Gui *World::Gui() const { - return this->dataPtr->gui.get(); + return optionalToPointer(this->dataPtr->gui); } ///////////////////////////////////////////////// void World::SetGui(const sdf::Gui &_gui) { - return this->dataPtr->gui.reset(new sdf::Gui(_gui)); + this->dataPtr->gui = _gui; } ///////////////////////////////////////////////// const sdf::Scene *World::Scene() const { - return this->dataPtr->scene.get(); + return optionalToPointer(this->dataPtr->scene); } ///////////////////////////////////////////////// void World::SetScene(const sdf::Scene &_scene) { - return this->dataPtr->scene.reset(new sdf::Scene(_scene)); + this->dataPtr->scene = _scene; } /////////////////////////////////////////////////