diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index 0eb800ca2..e8283ec84 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -16,7 +16,17 @@ jobs: - run: brew config - run: brew list + # Workaround for https://github.com/actions/setup-python/issues/577 + - name: Clean up python binaries + run: | + rm /usr/local/bin/2to3; + rm /usr/local/bin/idle3; + rm /usr/local/bin/pydoc3; + rm /usr/local/bin/python3; + rm /usr/local/bin/python3-config; - name: Install base dependencies + env: + HOMEBREW_NO_INSTALLED_DEPENDENTS_CHECK: 1 run: | brew tap osrf/simulation; # check for ci_matching_branch diff --git a/Changelog.md b/Changelog.md index 19b49c326..1c842d6b7 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,60 @@ ## libsdformat 13.X +### libsdformat 13.3.0 (2022-12-12) + +1. Sensor: add sdf::Errors output to API methods + * [Pull request #1138](https://github.com/gazebosim/sdformat/pull/1138) + +1. Warn child joint that resolves to world + * [Pull request #1211](https://github.com/gazebosim/sdformat/pull/1211) + +1. Converter: add sdf::Errors output to API methods + * [Pull request #1123](https://github.com/gazebosim/sdformat/pull/1123) + +1. ParamPassing: sdfwarns to sdf::Errors when warnings policy set to sdf::EnforcementPolicy::ERR + * [Pull request #1135](https://github.com/gazebosim/sdformat/pull/1135) + +1. Camera: added HasLensProjection + * [Pull request #1203](https://github.com/gazebosim/sdformat/pull/1203) + +1. Change default `camera_info_topic` value to `__default__` + * [Pull request #1201](https://github.com/gazebosim/sdformat/pull/1201) + +1. Check JointAxis expressed-in values during Load + * [Pull request #1195](https://github.com/gazebosim/sdformat/pull/1195) + +1. Added camera info topic to Camera + * [Pull request #1198](https://github.com/gazebosim/sdformat/pull/1198) + * [Pull request #1200](https://github.com/gazebosim/sdformat/pull/1200) + +1. Fix static URDF models with fixed joints + * [Pull request #1193](https://github.com/gazebosim/sdformat/pull/1193) + +### libsdformat 13.2.0 (2022-10-20) + +1. sdf/1.10/joint.sdf: add `screw_thread_pitch` + * [Pull request #1168](https://github.com/gazebosim/sdformat/pull/1168) + +1. sdf/1.10: support //world/joint specification + * [Pull request #1117](https://github.com/gazebosim/sdformat/pull/1117) + * [Pull request #1189](https://github.com/gazebosim/sdformat/pull/1189) + +1. Model: add sdf::Errors output to API methods + * [Pull request #1122](https://github.com/gazebosim/sdformat/pull/1122) + +1. Added Root::WorldByName + * [Pull request #1121](https://github.com/gazebosim/sdformat/pull/1121) + +1. Python: add OpticalFrameID APIs to pyCamera + * [Pull request #1184](https://github.com/gazebosim/sdformat/pull/1184) + +1. Fix `GZ_PYTHON_INSTALL_PATH` value + * [Pull request #1183](https://github.com/gazebosim/sdformat/pull/1183) + +1. Rename python bindings import library for Windows + * [Pull request #1165](https://github.com/gazebosim/sdformat/pull/1165) + ### libsdformat 13.1.0 (2022-10-12) 1. Add test helper python package for encapsulating versioned python packages @@ -1298,14 +1352,40 @@ ## libsdformat 9.X +### libsdformat 9.10.0 (2022-11-30) + +1. Ign to gz header migration. + * [Pull request #1118](https://github.com/gazebosim/sdformat/pull/1118) + +1. Added HasLensProjection. + * [Pull request #1203](https://github.com/gazebosim/sdformat/pull/1203) + +1. Added camera info topic to Camera + * [Pull request #1198](https://github.com/gazebosim/sdformat/pull/1198) + * [Pull request #1201](https://github.com/gazebosim/sdformat/pull/1201) + +### libsdformat 9.9.1 (2022-11-08) + +1. Fix static URDF models with fixed joints + * [Pull request #1193](https://github.com/gazebosim/sdformat/pull/1193) + +1. Don't assume `CMAKE_INSTALL_*DIR` variables are relative + * [Pull request #1190](https://github.com/gazebosim/sdformat/pull/1190) + ### libsdformat 9.9.0 (2022-09-07) 1. sdf/camera.sdf: fields for projection matrix * [Pull request #1088](https://github.com/gazebosim/sdformat/pull/1088) +1. urdf: add //frame for reduced links/joints + * [Pull request #1148](https://github.com/gazebosim/sdformat/pull/1148) + 1. urdf: fix sensor/light pose for links lumped by fixed joints * [Pull request #1114](https://github.com/gazebosim/sdformat/pull/1114) +1. urdf: fix test and clean up internals + * [Pull request #1126](https://github.com/gazebosim/sdformat/pull/1126) + 1. Ensure relocatable config files * [Pull request #419](https://github.com/gazebosim/sdformat/pull/419) * [Pull request #1093](https://github.com/gazebosim/sdformat/pull/1093) diff --git a/Migration.md b/Migration.md index 513746002..5fbe44b91 100644 --- a/Migration.md +++ b/Migration.md @@ -555,6 +555,10 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. ## SDFormat specification 1.9 to 1.10 +### Additions + +1. **world.sdf**: A joint can be specified directly in a world. + ### Modifications 1. **joint.sdf**: axis limits default values have changed @@ -563,6 +567,8 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. + `//limit/velocity`: `inf` (formerly `-1`) + `//limit/effort`: `inf` (formerly `-1`) +1. **joint.sdf**: thread_pitch is deprecated in favor of screw_thread_pitch. + 1. **plugin.sdf**: name attribute is now optional with empty default value. ### Removals diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 8fb5260be..5c46827c8 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -81,6 +81,14 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Get the camera info topic + /// \return Topic for the camera info + public: std::string CameraInfoTopic() const; + + /// \brief Set the camera info topic + /// \param[in] _cameraInfoTopic Topic for the camera info. + public: void SetCameraInfoTopic(const std::string& _cameraInfoTopic); + /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has @@ -558,6 +566,10 @@ namespace sdf /// \return True if the camera has instrinsics values set, false otherwise public: bool HasLensIntrinsics() const; + /// \brief Get whether or not the camera has projection values set + /// \return True if the camera has projection values set, false otherwise + public: bool HasLensProjection() const; + /// \brief Create and return an SDF element filled with data from this /// camera. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index b3c58f2a1..096f7ee73 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -168,6 +168,16 @@ namespace sdf /// \brief Generic warning saved as error due to WarningsPolicy config WARNING, + + /// \brief The joint axis expressed-in value does not match the name of an + /// existing frame in the current scope. + JOINT_AXIS_EXPRESSED_IN_INVALID, + + /// \brief SDF conversion generic error. + CONVERSION_ERROR, + + /// \brief Generic error during parsing. + PARSING_ERROR, }; class SDFORMAT_VISIBLE Error diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 73764b7e7..26c4b226f 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -203,12 +203,27 @@ namespace sdf /// \param[in] _frame The name of the pose relative-to frame. public: void SetPoseRelativeTo(const std::string &_frame); - /// \brief Get the thread pitch (only valid for screw joints) - /// \return The thread pitch + /// \brief Get the displacement along the joint axis for each complete + /// revolution around the joint axis (only valid for screw joints). + /// \return The thread pitch with units of meters per revolution with a + /// positive value corresponding to a right-handed thread. + public: double ScrewThreadPitch() const; + + /// \brief Set the thread pitch (only valid for screw joints). + /// \param[in] _threadPitch The thread pitch with units of meters per + /// revolution with a positive value corresponding to a right-handed thread. + public: void SetScrewThreadPitch(double _threadPitch); + + /// \brief Get the thread pitch in gazebo-classic format (only valid for + /// screw joints). This will be deprecated in a future version. + /// \return The thread pitch with units of radians / meters and a positive + /// value coresponding to a left-handed thread. public: double ThreadPitch() const; - /// \brief Set the thread pitch (only valid for screw joints) - /// \param[in] _threadPitch The thread pitch of the joint + /// \brief Set the thread pitch in gazebo-classic format (only valid for + /// screw joints). This will be deprecated in a future version. + /// \param[in] _threadPitch The thread pitch with units of radians / meters + /// and a positive value coresponding to a left-handed thread. public: void SetThreadPitch(double _threadPitch); /// \brief Get a pointer to the SDF element that was used during @@ -291,6 +306,9 @@ namespace sdf /// \brief Allow Model::Load to call SetPoseRelativeToGraph. friend class Model; + /// \brief Allow World::Load to call SetPoseRelativeToGraph. + friend class World; + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index af17a8e76..ad8195fe5 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -166,14 +166,17 @@ class SDFORMAT_VISIBLE ParserConfig public: void RegisterCustomModelParser(CustomModelParser _modelParser); /// \brief Get the registered custom model parsers + /// \return Vector of registered model parser callbacks. public: const std::vector &CustomModelParsers() const; /// \brief Set the preserveFixedJoint flag. - /// \param[in] _preserveFixedJoint flag value to set + /// \param[in] _preserveFixedJoint True to preserve fixed joints, false to + /// reduce the fixed joints and merge the child link into the parent. public: void URDFSetPreserveFixedJoint(bool _preserveFixedJoint); /// \brief Get the preserveFixedJoint flag value. - /// \return Current flag value + /// \return True to preserve fixed joints, false to reduce the fixed joints + /// and merge the child link into the parent. public: bool URDFPreserveFixedJoint() const; /// \brief Set the storeResolvedURIs flag value. diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 11e14502c..1d88defcc 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -146,6 +146,20 @@ namespace sdf /// \sa uint64_t WorldCount() const public: World *WorldByIndex(const uint64_t _index); + /// \brief Get a world based on a name. + /// \param[in] _name Name of the world. + /// \return Pointer to the world. Nullptr if a world with the given name + /// does not exist. + /// \sa bool WorldNameExists(const std::string &_name) const + public: const World *WorldByName(const std::string &_name) const; + + /// \brief Get a world based on a name. + /// \param[in] _name Name of the world. + /// \return Pointer to the world. Nullptr if a world with the given name + /// does not exist. + /// \sa bool WorldNameExists(const std::string &_name) const + public: World *WorldByName(const std::string &_name); + /// \brief Get whether a world name exists. /// \param[in] _name Name of the world to check. /// \return True if there exists a world with the given name. diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index b8733d2d4..b658917d6 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -214,6 +214,14 @@ namespace sdf /// \return SDF element pointer with updated sensor values. public: sdf::ElementPtr ToElement() const; + /// \brief Create and return an SDF element filled with data from this + /// sensor. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[in] _errors Vector of errors. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const; + /// \brief Get the sensor type. /// \return The sensor type. public: SensorType Type() const; diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 8d1116d33..0afc85472 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -45,6 +45,7 @@ namespace sdf class Actor; class Frame; class InterfaceModel; + class Joint; class Light; class Model; class ParserConfig; @@ -212,6 +213,12 @@ namespace sdf /// exists. public: bool AddActor(const Actor &_actor); + /// \brief Add a joint to the world. + /// \param[in] _joint Joint to add. + /// \return True if successful, false if a joint with the name already + /// exists. + public: bool AddJoint(const Joint &_joint); + /// \brief Add a light to the world. /// \param[in] _light Light to add. /// \return True if successful, false if a light with the name already @@ -233,10 +240,13 @@ namespace sdf /// \brief Remove all models. public: void ClearModels(); - /// \brief Remove all models. + /// \brief Remove all actors. public: void ClearActors(); - /// \brief Remove all models. + /// \brief Remove all joints. + public: void ClearJoints(); + + /// \brief Remove all lights. public: void ClearLights(); /// \brief Remove all physics. @@ -316,6 +326,45 @@ namespace sdf /// \return True if there exists an explicit frame with the given name. public: bool FrameNameExists(const std::string &_name) const; + /// \brief Get the number of joints. + /// \return Number of joints contained in this World object. + public: uint64_t JointCount() const; + + /// \brief Get a joint based on an index. + /// \param[in] _index Index of the joint. The index should be in the + /// range [0..JointCount()). + /// \return Pointer to the joint. Nullptr if the index does not exist. + /// \sa uint64_t JointCount() const + public: const Joint *JointByIndex(uint64_t _index) const; + + /// \brief Get a mutable joint based on an index. + /// \param[in] _index Index of the joint. The index should be in the + /// range [0..JointCount()). + /// \return Pointer to the joint. Nullptr if the index does not exist. + /// \sa uint64_t JointCount() const + public: Joint *JointByIndex(uint64_t _index); + + /// \brief Get an joint based on a name. + /// \param[in] _name Name of the joint. + /// To get a joint in a nested model, prefix the joint name with the + /// sequence of nested models containing this joint, delimited by "::". + /// \return Pointer to the joint. Nullptr if the name does not + /// exist. + public: const Joint *JointByName(const std::string &_name) const; + + /// \brief Get a mutable joint based on a name. + /// \param[in] _name Name of the joint. + /// To get a joint in a nested model, prefix the joint name with the + /// sequence of nested models containing this joint, delimited by "::". + /// \return Pointer to the joint. Nullptr if the name does not + /// exist. + public: Joint *JointByName(const std::string &_name); + + /// \brief Get whether a joint name exists. + /// \param[in] _name Name of the joint to check. + /// \return True if there exists a joint with the given name. + public: bool JointNameExists(const std::string &_name) const; + /// \brief Get the number of lights. /// \return Number of lights contained in this World object. public: uint64_t LightCount() const; @@ -438,6 +487,18 @@ namespace sdf public: sdf::ElementPtr ToElement( const OutputConfig &_config = OutputConfig::GlobalConfig()) const; + /// \brief Check if a given name exists in the FrameAttachedTo graph at the + /// scope of the world. + /// \param[in] _name Name of the implicit or explicit frame to check. + /// To check for a frame in a nested model, prefix the frame name with + /// the sequence of nested models containing this frame, delimited by "::". + /// \return True if the frame name is found in the FrameAttachedTo graph. + /// False otherwise, or if the frame graph is invalid. + /// \note This function assumes the world has a valid FrameAttachedTo graph. + /// It will return false if the graph is invalid. + public: bool NameExistsInFrameAttachedToGraph( + const std::string &_name) const; + /// \brief Get the plugins attached to this object. /// \return A vector of Plugin, which will be empty if there are no /// plugins. diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index 03df56462..2db2a7e44 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -38,6 +38,8 @@ namespace sdf // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // + // TODO(marcoag): Deprecate function overloads that do not use sdf::Errors, + // see: https://github.com/gazebosim/sdformat/issues/1186 class Root; /// \brief Initialize the SDF interface from the embedded root spec file @@ -334,6 +336,17 @@ namespace sdf bool convertFile(const std::string &_filename, const std::string &_version, const ParserConfig &_config, SDFPtr _sdf); + /// \brief Convert an SDF file to a specific SDF version. + /// \param[out] _sdf Pointer to the converted SDF document. + /// \param[in] _filename Name of the SDF file to convert. + /// \param[in] _version Version to convert _filename to. + /// \param[in] _config Custom parser configuration + /// \return Vector of errors, successful when vector is empty. + SDFORMAT_VISIBLE + sdf::Errors convertFile( + SDFPtr _sdf, const std::string &_filename, const std::string &_version, + const ParserConfig &_config = ParserConfig::GlobalConfig()); + /// \brief Convert an SDF string to a specific SDF version. /// \param[in] _sdfString The SDF string to convert. /// \param[in] _version Version to convert _filename to. @@ -353,6 +366,17 @@ namespace sdf bool convertString(const std::string &_sdfString, const std::string &_version, const ParserConfig &_config, SDFPtr _sdf); + /// \brief Convert an SDF string to a specific SDF version. + /// \param[out] _sdf Pointer to the converted SDF document. + /// \param[in] _sdfString The SDF string to convert. + /// \param[in] _version Version to convert _filename to. + /// \param[in] _config Custom parser configuration + /// \return Vector of errors, successful when vector is empty. + SDFORMAT_VISIBLE + sdf::Errors convertString( + SDFPtr _sdf, const std::string &_sdfString, const std::string &_version, + const ParserConfig &_config = ParserConfig::GlobalConfig()); + /// \brief Check that for each model, the canonical_link attribute value /// matches the name of a link in the model if the attribute is set and /// not empty. @@ -416,6 +440,15 @@ namespace sdf SDFORMAT_VISIBLE void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors); + /// \brief Check that all joint axes in contained joints specify xyz + /// expressed-in names that match the names of valid frames. + /// This checks recursively and should check the files exhaustively + /// rather than terminating early when the first error is found. + /// \param[in] _root SDF Root object to check recursively. + /// \param[out] _errors Detected errors will be appended to this variable. + SDFORMAT_VISIBLE + void checkJointAxisExpressedInValues(const sdf::Root *_root, Errors &_errors); + /// \brief For the world and each model, check that the attached_to graphs /// build without errors and have no cycles. /// Confirm that following directed edges from each vertex in the graph diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e536c749f..c096a1dd9 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -22,6 +22,9 @@ if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION) if(USE_DIST_PACKAGES_FOR_PYTHON) string(REPLACE "site-packages" "dist-packages" GZ_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) + else() + # custom cmake command is returning dist-packages + string(REPLACE "dist-packages" "site-packages" GZ_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) endif() else() # If not a system installation, respect local paths @@ -98,6 +101,12 @@ target_compile_definitions(${BINDINGS_MODULE_NAME} PRIVATE set_target_properties(${BINDINGS_MODULE_NAME} PROPERTIES OUTPUT_NAME "sdformat${PROJECT_VERSION_MAJOR}") +# Deal with naming collision on Windows. It is caused by the both the import +# library of sdformat library and the import library of the pybind11 bindings +# library are called sdformat13.lib. +# See https://github.com/gazebosim/sdformat/issues/1150 +set_target_properties(${BINDINGS_MODULE_NAME} PROPERTIES + ARCHIVE_OUTPUT_NAME "python_sdformat${PROJECT_VERSION_MAJOR}") configure_build_install_location(${BINDINGS_MODULE_NAME}) diff --git a/python/src/sdf/pyCamera.cc b/python/src/sdf/pyCamera.cc index 842ec23d7..e91683d5e 100644 --- a/python/src/sdf/pyCamera.cc +++ b/python/src/sdf/pyCamera.cc @@ -42,6 +42,10 @@ void defineCamera(pybind11::object module) "Get the name of the camera.") .def("set_name", &sdf::Camera::SetName, "Set the name of the camera.") + .def("set_camera_info_topic", &sdf::Camera::SetCameraInfoTopic, + "Set the camera info topic") + .def("camera_info_topic", &sdf::Camera::CameraInfoTopic, + "Get the camera info topic.") .def("triggered", &sdf::Camera::Triggered, "Get whether the camera is triggered by a topic.") .def("set_triggered", &sdf::Camera::SetTriggered, @@ -171,6 +175,16 @@ void defineCamera(pybind11::object module) "Set the name of the coordinate frame relative to which this " "object's pose is expressed. An empty value indicates that the frame " "is relative to the parent link.") + .def("optical_frame_id", &sdf::Camera::OpticalFrameId, + "Get the name of the coordinate frame relative to which this " + "object's camera_info message header is expressed. " + "Note: while Gazebo interprets the camera frame to be looking towards " + "+X, other tools, such as ROS interprets this frame as looking " + "towards +Z. The Camera sensor assumes that the color and depth " + "images are captured at the same frame_id.") + .def("set_optical_frame_id", &sdf::Camera::SetOpticalFrameId, + "Set the name of the coordinate frame relative to which this " + "object's camera_info is expressed.") .def("lens_type", &sdf::Camera::LensType, "Get the lens type. This is the type of the lens mapping. " "Supported values are gnomonical, stereographic, equidistant, " @@ -253,6 +267,8 @@ void defineCamera(pybind11::object module) "Set the visibility mask of a camera") .def("has_lens_intrinsics", &sdf::Camera::HasLensIntrinsics, "Get whether or not the camera has instrinsics values set") + .def("has_lens_projection", &sdf::Camera::HasLensProjection, + "Get whether or not the camera has proejction values set") .def("__copy__", [](const sdf::Camera &self) { return sdf::Camera(self); }) diff --git a/python/src/sdf/pyJoint.cc b/python/src/sdf/pyJoint.cc index 3ea9b48e6..4d9199b93 100644 --- a/python/src/sdf/pyJoint.cc +++ b/python/src/sdf/pyJoint.cc @@ -92,10 +92,18 @@ void defineJoint(pybind11::object module) "Set the name of the coordinate frame relative to which this " "object's pose is expressed. An empty value indicates that the frame " "is relative to the child link frame.") + .def("screw_thread_pitch", &sdf::Joint::ScrewThreadPitch, + "Get the thread pitch in meters per revolution with a positive value " + "for right-handed threads (only valid for screw joints)") + .def("set_screw_thread_pitch", &sdf::Joint::SetScrewThreadPitch, + "Set the thread pitch in meters per revolution with a positive value " + "for right-handed threads (only valid for screw joints)") .def("thread_pitch", &sdf::Joint::ThreadPitch, - "Get the thread pitch (only valid for screw joints)") + "Get the thread pitch in gazebo-classic format (only valid for screw " + "joints)") .def("set_thread_pitch", &sdf::Joint::SetThreadPitch, - "Set the thread pitch (only valid for screw joints)") + "Get the thread pitch in gazebo-classic format (only valid for screw " + "joints)") .def("semantic_pose", &sdf::Joint::SemanticPose, "Get SemanticPose object of this object to aid in resolving " "poses.") diff --git a/python/src/sdf/pyModel.cc b/python/src/sdf/pyModel.cc index e38d3b17c..fce94d0b9 100644 --- a/python/src/sdf/pyModel.cc +++ b/python/src/sdf/pyModel.cc @@ -175,7 +175,7 @@ void defineModel(pybind11::object module) .def("name_exists_in_frame_attached_to_graph", &sdf::Model::NameExistsInFrameAttachedToGraph, "Check if a given name exists in the FrameAttachedTo graph at the " - "scope of the model..") + "scope of the model.") .def("add_link", &sdf::Model::AddLink, "Add a link to the model.") .def("add_joint", &sdf::Model::AddJoint, diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index c82fd2dc4..41b930fc4 100644 --- a/python/src/sdf/pyWorld.cc +++ b/python/src/sdf/pyWorld.cc @@ -107,6 +107,10 @@ void defineWorld(pybind11::object module) "index.") .def("model_name_exists", &sdf::World::ModelNameExists, "Get whether a model name exists.") + .def("name_exists_in_frame_attached_to_graph", + &sdf::World::NameExistsInFrameAttachedToGraph, + "Check if a given name exists in the FrameAttachedTo graph at the " + "scope of the world.") .def("add_model", &sdf::World::AddModel, "Add a model to the world.") // .def("add_actor", &sdf::World::AddActor, @@ -117,6 +121,8 @@ void defineWorld(pybind11::object module) "Add a physics object to the world.") .def("add_frame", &sdf::World::AddFrame, "Add a frame object to the world.") + .def("add_joint", &sdf::World::AddJoint, + "Add a joint to the world.") .def("clear_models", &sdf::World::ClearModels, "Remove all models.") // .def("clear_actors", &sdf::World::ClearActors, @@ -127,6 +133,8 @@ void defineWorld(pybind11::object module) "Remove all physics objects.") .def("clear_frames", &sdf::World::ClearFrames, "Remove all frames.") + .def("clear_joints", &sdf::World::ClearJoints, + "Remove all joints.") .def("actor_count", &sdf::World::ActorCount, "Get the number of actors.") .def("frame_count", &sdf::World::FrameCount, @@ -145,6 +153,20 @@ void defineWorld(pybind11::object module) "index.") .def("frame_name_exists", &sdf::World::FrameNameExists, "Get whether a frame name exists.") + .def("joint_count", &sdf::World::JointCount, + "Get the number of joints.") + .def("joint_by_index", + pybind11::overload_cast( + &sdf::World::JointByIndex), + pybind11::return_value_policy::reference_internal, + "Get a mutable joint based on an index.") + .def("joint_by_name", + pybind11::overload_cast( + &sdf::World::JointByName), + pybind11::return_value_policy::reference_internal, + "Get a mutable joint based on name.") + .def("joint_name_exists", &sdf::World::JointNameExists, + "Get whether a joint name exists.") .def("light_count", &sdf::World::LightCount, "Get the number of lights.") .def("light_by_index", diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index 40000b7f0..6daca4cbe 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -35,6 +35,10 @@ def test_default_construction(self): cam.set_trigger_topic("my_camera/trigger") self.assertEqual("my_camera/trigger", cam.trigger_topic()) + self.assertEqual("", cam.camera_info_topic()); + cam.set_camera_info_topic("/camera/camera_info"); + self.assertEqual("/camera/camera_info", cam.camera_info_topic()); + self.assertAlmostEqual(1.047, cam.horizontal_fov().radian()) cam.set_horizontal_fov(Angle(1.45)) self.assertAlmostEqual(1.45, cam.horizontal_fov().radian()) @@ -131,6 +135,10 @@ def test_default_construction(self): cam.set_pose_relative_to("/frame") self.assertEqual("/frame", cam.pose_relative_to()) + self.assertFalse(cam.optical_frame_id()); + cam.set_optical_frame_id("/optical_frame"); + self.assertEqual("/optical_frame", cam.optical_frame_id()); + self.assertEqual("stereographic", cam.lens_type()) cam.set_lens_type("custom") self.assertEqual("custom", cam.lens_type()) @@ -188,6 +196,7 @@ def test_default_construction(self): self.assertAlmostEqual(2.3, cam.lens_intrinsics_skew()) self.assertTrue(cam.has_lens_intrinsics()) + self.assertFalse(cam.has_lens_projection()) self.assertEqual(4294967295, cam.visibility_mask()) cam.set_visibility_mask(123) diff --git a/python/test/pyJoint_TEST.py b/python/test/pyJoint_TEST.py index f8577b93c..578fc4145 100644 --- a/python/test/pyJoint_TEST.py +++ b/python/test/pyJoint_TEST.py @@ -98,10 +98,16 @@ def test_default_construction(self): self.assertEqual(axis.xyz(), joint.axis(0).xyz()) self.assertEqual(axis1.xyz(), joint.axis(1).xyz()) - self.assertAlmostEqual(1.0, joint.thread_pitch()) + # Default thread pitch + self.assertAlmostEqual(1.0, joint.screw_thread_pitch()) + self.assertAlmostEqual(-2*math.pi, joint.thread_pitch()) threadPitch = 0.1 + joint.set_screw_thread_pitch(threadPitch) + self.assertAlmostEqual(threadPitch, joint.screw_thread_pitch()) + self.assertAlmostEqual(-2*math.pi / threadPitch, joint.thread_pitch()) joint.set_thread_pitch(threadPitch) self.assertAlmostEqual(threadPitch, joint.thread_pitch()) + self.assertAlmostEqual(-2*math.pi / threadPitch, joint.screw_thread_pitch()) self.assertEqual(0, joint.sensor_count()) self.assertEqual(None, joint.sensor_by_index(0)) diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py index ce8d7e5b4..126c1b579 100644 --- a/python/test/pyModel_TEST.py +++ b/python/test/pyModel_TEST.py @@ -145,6 +145,10 @@ def test_default_construction(self): self.assertEqual(errors[1].message(), "PoseRelativeToGraph error: scope does not point to a valid graph.") + # model doesn't have graphs, so no names should exist in graphs + self.assertFalse(model.name_exists_in_frame_attached_to_graph("")); + self.assertFalse(model.name_exists_in_frame_attached_to_graph("link")); + def test_copy_construction(self): model = Model() diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 4048a349d..08c62ab12 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -15,7 +15,7 @@ import copy from gz_test_deps.math import Color, Vector3d, SphericalCoordinates from gz_test_deps.sdformat import (Atmosphere, Gui, Physics, Plugin, Error, - Frame, Light, Model, Scene, World) + Frame, Joint, Light, Model, Scene, World) import gz_test_deps.sdformat as sdf import unittest import math @@ -52,13 +52,28 @@ def test_default_construction(self): self.assertEqual(None, world.frame_by_index(0)) self.assertEqual(None, world.frame_by_index(1)) self.assertFalse(world.frame_name_exists("")) - self.assertFalse(world.frame_name_exists("default")) - - self.assertEqual(0, world.frame_count()) - self.assertEqual(None, world.frame_by_index(0)) - self.assertEqual(None, world.frame_by_index(1)) - self.assertFalse(world.frame_name_exists("")) - self.assertFalse(world.frame_name_exists("default")) + self.assertFalse(world.frame_name_exists("a::b")) + self.assertFalse(world.frame_name_exists("a::b::c")) + self.assertFalse(world.frame_name_exists("::::")) + self.assertEqual(None, world.frame_by_name("")) + self.assertEqual(None, world.frame_by_name("default")) + self.assertEqual(None, world.frame_by_name("a::b")) + self.assertEqual(None, world.frame_by_name("a::b::c")) + self.assertEqual(None, world.frame_by_name("::::")) + + self.assertEqual(0, world.joint_count()) + self.assertEqual(None, world.joint_by_index(0)) + self.assertEqual(None, world.joint_by_index(1)) + self.assertFalse(world.joint_name_exists("")) + self.assertFalse(world.joint_name_exists("default")) + self.assertFalse(world.joint_name_exists("a::b")) + self.assertFalse(world.joint_name_exists("a::b::c")) + self.assertFalse(world.joint_name_exists("::::")) + self.assertEqual(None, world.joint_by_name("")) + self.assertEqual(None, world.joint_by_name("default")) + self.assertEqual(None, world.joint_by_name("a::b")) + self.assertEqual(None, world.joint_by_name("a::b::c")) + self.assertEqual(None, world.joint_by_name("::::")) self.assertEqual(1, world.physics_count()) @@ -71,6 +86,10 @@ def test_default_construction(self): self.assertEqual(errors[1].message(), "PoseRelativeToGraph error: scope does not point to a valid graph.") + # world doesn't have graphs, so no names should exist in graphs + self.assertFalse(world.name_exists_in_frame_attached_to_graph("")); + self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")); + def test_copy_construction(self): world = World() @@ -318,6 +337,10 @@ def test_mutable_by_index(self): frame.set_name("frame1") self.assertTrue(world.add_frame(frame)) + joint = Joint() + joint.set_name("joint1") + self.assertTrue(world.add_joint(joint)) + # Modify the model m = world.model_by_index(0) self.assertNotEqual(None, m) @@ -353,6 +376,13 @@ def test_mutable_by_index(self): f.set_name("frame2") self.assertEqual("frame2", world.frame_by_index(0).name()) + # Modify the joint + j = world.joint_by_index(0) + self.assertNotEqual(None, j) + self.assertEqual("joint1", j.name()) + j.set_name("joint2") + self.assertEqual("joint2", world.joint_by_index(0).name()) + def test_mutable_by_name(self): world = World() @@ -364,6 +394,10 @@ def test_mutable_by_name(self): frame.set_name("frame1") self.assertTrue(world.add_frame(frame)) + joint = Joint() + joint.set_name("joint1") + self.assertTrue(world.add_joint(joint)) + # Modify the model m = world.model_by_name("model1") self.assertNotEqual(None, m) @@ -380,6 +414,14 @@ def test_mutable_by_name(self): self.assertFalse(world.frame_by_name("frame1")) self.assertTrue(world.frame_by_name("frame2")) + # Modify the joint + j = world.joint_by_name("joint1") + self.assertNotEqual(None, j) + self.assertEqual("joint1", j.name()) + j.set_name("joint2") + self.assertFalse(world.joint_by_name("joint1")) + self.assertTrue(world.joint_by_name("joint2")) + def test_plugins(self): world = World() self.assertEqual(0, len(world.plugins())) diff --git a/sdf/1.10/camera.sdf b/sdf/1.10/camera.sdf index 708e1d6eb..08f406218 100644 --- a/sdf/1.10/camera.sdf +++ b/sdf/1.10/camera.sdf @@ -9,6 +9,10 @@ If the camera will be triggered by a topic + + Name of the camera info + + Name of the topic that will trigger the camera if enabled diff --git a/sdf/1.10/joint.sdf b/sdf/1.10/joint.sdf index 04d5f52e1..b757614e9 100644 --- a/sdf/1.10/joint.sdf +++ b/sdf/1.10/joint.sdf @@ -3,7 +3,7 @@ A joint connects two links with kinematic and dynamic properties. By default, the pose of a joint is expressed in the child link frame. - A unique name for the joint within the scope of the model. + A unique name for the joint within its scope. @@ -36,8 +36,24 @@ Parameter for gearbox joints. Gearbox ratio is enforced over two joint angles. First joint angle (theta_1) is the angle from the gearbox_reference_body to the parent link in the direction of the axis element and the second joint angle (theta_2) is the angle from the gearbox_reference_body to the child link in the direction of the axis2 element. - - Parameter for screw joints. + + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + The parameter is now deprecated in favor of `screw_thread_pitch`. + + + + + + A parameter for screw joint kinematics, representing the + axial distance traveled for each revolution of the joint, + with units of meters / revolution with a positive value corresponding + to a right-handed thread. + This parameter supersedes `thread_pitch`. + diff --git a/sdf/1.10/world.sdf b/sdf/1.10/world.sdf index 0f3efe585..07dafc08e 100644 --- a/sdf/1.10/world.sdf +++ b/sdf/1.10/world.sdf @@ -60,6 +60,7 @@ + diff --git a/sdf/1.4/joint.sdf b/sdf/1.4/joint.sdf index 4ff4e1137..b919223ad 100644 --- a/sdf/1.4/joint.sdf +++ b/sdf/1.4/joint.sdf @@ -31,7 +31,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.5/joint.sdf b/sdf/1.5/joint.sdf index d6719b4a7..c8c9cee50 100644 --- a/sdf/1.5/joint.sdf +++ b/sdf/1.5/joint.sdf @@ -36,7 +36,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.6/joint.sdf b/sdf/1.6/joint.sdf index 21bfb7d97..33538429e 100644 --- a/sdf/1.6/joint.sdf +++ b/sdf/1.6/joint.sdf @@ -37,7 +37,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.7/camera.sdf b/sdf/1.7/camera.sdf index ec0bcafaa..9e05cfbda 100644 --- a/sdf/1.7/camera.sdf +++ b/sdf/1.7/camera.sdf @@ -5,6 +5,10 @@ An optional name for the camera. + + Name of the camera info + + Horizontal field of view diff --git a/sdf/1.7/joint.sdf b/sdf/1.7/joint.sdf index b4a94b0f6..9d704af27 100644 --- a/sdf/1.7/joint.sdf +++ b/sdf/1.7/joint.sdf @@ -37,7 +37,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.8/camera.sdf b/sdf/1.8/camera.sdf index ec0bcafaa..fb33b088a 100644 --- a/sdf/1.8/camera.sdf +++ b/sdf/1.8/camera.sdf @@ -5,6 +5,10 @@ An optional name for the camera. + + Name of the camera info + + Horizontal field of view diff --git a/sdf/1.8/joint.sdf b/sdf/1.8/joint.sdf index efea1d1ec..d64981657 100644 --- a/sdf/1.8/joint.sdf +++ b/sdf/1.8/joint.sdf @@ -37,7 +37,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.9/camera.sdf b/sdf/1.9/camera.sdf index 708e1d6eb..3c7f6ee95 100644 --- a/sdf/1.9/camera.sdf +++ b/sdf/1.9/camera.sdf @@ -5,6 +5,10 @@ An optional name for the camera. + + Name of the camera info + + If the camera will be triggered by a topic diff --git a/sdf/1.9/joint.sdf b/sdf/1.9/joint.sdf index d472feefa..2ebea2c71 100644 --- a/sdf/1.9/joint.sdf +++ b/sdf/1.9/joint.sdf @@ -37,7 +37,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 66affa10a..feab8e716 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -57,6 +57,7 @@ if (BUILD_TESTING) target_sources(UNIT_Converter_TEST PRIVATE Converter.cc EmbeddedSdf.cc + Utils.cc XmlUtils.cc) endif() diff --git a/src/Camera.cc b/src/Camera.cc index 1e63bcece..c41850a0d 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -52,6 +52,9 @@ class sdf::Camera::Implementation /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; + /// \brief Camera info topic. + public: std::string cameraInfoTopic = ""; + /// \brief Name of the camera. public: std::string name = ""; @@ -208,6 +211,9 @@ class sdf::Camera::Implementation /// \brief True if this camera has custom intrinsics values public: bool hasIntrinsics = false; + /// \brief True if this camera has custom projection values + public: bool hasProjection = false; + /// \brief Visibility mask of a camera. Defaults to 0xFFFFFFFF public: uint32_t visibilityMask{UINT32_MAX}; }; @@ -252,6 +258,11 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->triggerTopic = _sdf->Get("trigger_topic", this->dataPtr->triggerTopic).first; + this->dataPtr->cameraInfoTopic = _sdf->Get("camera_info_topic", + this->dataPtr->cameraInfoTopic).first; + if (this->dataPtr->cameraInfoTopic == "__default__") + this->dataPtr->cameraInfoTopic = ""; + this->dataPtr->hfov = _sdf->Get("horizontal_fov", this->dataPtr->hfov).first; @@ -435,7 +446,7 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->lensProjectionTx).first; this->dataPtr->lensProjectionTy = projection->Get("ty", this->dataPtr->lensProjectionTy).first; - + this->dataPtr->hasProjection = true; } } @@ -448,6 +459,18 @@ Errors Camera::Load(ElementPtr _sdf) return errors; } +///////////////////////////////////////////////// +std::string Camera::CameraInfoTopic() const +{ + return this->dataPtr->cameraInfoTopic; +} + +///////////////////////////////////////////////// +void Camera::SetCameraInfoTopic(const std::string &_cameraInfoTopic) +{ + this->dataPtr->cameraInfoTopic = _cameraInfoTopic; +} + ///////////////////////////////////////////////// sdf::ElementPtr Camera::Element() const { @@ -1037,6 +1060,7 @@ double Camera::LensProjectionFx() const void Camera::SetLensProjectionFx(double _fx_p) { this->dataPtr->lensProjectionFx = _fx_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1049,6 +1073,7 @@ double Camera::LensProjectionFy() const void Camera::SetLensProjectionFy(double _fy_p) { this->dataPtr->lensProjectionFy = _fy_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1061,6 +1086,7 @@ double Camera::LensProjectionCx() const void Camera::SetLensProjectionCx(double _cx_p) { this->dataPtr->lensProjectionCx = _cx_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1073,6 +1099,7 @@ double Camera::LensProjectionCy() const void Camera::SetLensProjectionCy(double _cy_p) { this->dataPtr->lensProjectionCy = _cy_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1085,6 +1112,7 @@ double Camera::LensProjectionTx() const void Camera::SetLensProjectionTx(double _tx) { this->dataPtr->lensProjectionTx = _tx; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1097,6 +1125,7 @@ double Camera::LensProjectionTy() const void Camera::SetLensProjectionTy(double _ty) { this->dataPtr->lensProjectionTy = _ty; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1172,6 +1201,12 @@ bool Camera::HasLensIntrinsics() const return this->dataPtr->hasIntrinsics; } +///////////////////////////////////////////////// +bool Camera::HasLensProjection() const +{ + return this->dataPtr->hasProjection; +} + ///////////////////////////////////////////////// sdf::ElementPtr Camera::ToElement() const { @@ -1194,6 +1229,12 @@ sdf::ElementPtr Camera::ToElement() const imageElem->GetElement("format")->Set(this->PixelFormatStr()); imageElem->GetElement("anti_aliasing")->Set( this->AntiAliasingValue()); + elem->GetElement("camera_info_topic")->Set( + this->CameraInfoTopic()); + elem->GetElement("trigger_topic")->Set( + this->TriggerTopic()); + elem->GetElement("triggered")->Set( + this->Triggered()); sdf::ElementPtr clipElem = elem->GetElement("clip"); clipElem->GetElement("near")->Set(this->NearClip()); @@ -1272,6 +1313,16 @@ sdf::ElementPtr Camera::ToElement() const intrinsicsElem->GetElement("cy")->Set(this->LensIntrinsicsCy()); intrinsicsElem->GetElement("s")->Set(this->LensIntrinsicsSkew()); } + if (this->HasLensProjection()) + { + sdf::ElementPtr projectionElem = lensElem->GetElement("projection"); + projectionElem->GetElement("p_fx")->Set(this->LensProjectionFx()); + projectionElem->GetElement("p_fy")->Set(this->LensProjectionFy()); + projectionElem->GetElement("p_cx")->Set(this->LensProjectionCx()); + projectionElem->GetElement("p_cy")->Set(this->LensProjectionCy()); + projectionElem->GetElement("tx")->Set(this->LensProjectionTx()); + projectionElem->GetElement("ty")->Set(this->LensProjectionTy()); + } if (this->HasSegmentationType()) { diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index c10b6b05d..68b61beac 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -34,6 +34,10 @@ TEST(DOMCamera, Construction) cam.SetTriggerTopic("my_camera/trigger"); EXPECT_EQ("my_camera/trigger", cam.TriggerTopic()); + EXPECT_EQ("", cam.CameraInfoTopic()); + cam.SetCameraInfoTopic("/camera/camera_info"); + EXPECT_EQ("/camera/camera_info", cam.CameraInfoTopic()); + EXPECT_DOUBLE_EQ(1.047, cam.HorizontalFov().Radian()); cam.SetHorizontalFov(1.45); EXPECT_DOUBLE_EQ(1.45, cam.HorizontalFov().Radian()); @@ -215,6 +219,7 @@ TEST(DOMCamera, Construction) EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew()); EXPECT_TRUE(cam.HasLensIntrinsics()); + EXPECT_TRUE(cam.HasLensProjection()); EXPECT_EQ(UINT32_MAX, cam.VisibilityMask()); cam.SetVisibilityMask(123u); @@ -269,6 +274,9 @@ TEST(DOMCamera, ToElement) cam.SetSaveFrames(true); cam.SetSaveFramesPath("/tmp"); cam.SetOpticalFrameId("/optical_frame"); + cam.SetCameraInfoTopic("/camera_info_test"); + cam.SetTriggerTopic("/trigger_topic_test"); + cam.SetTriggered(true); sdf::ElementPtr camElem = cam.ToElement(); EXPECT_NE(nullptr, camElem); @@ -289,6 +297,9 @@ TEST(DOMCamera, ToElement) EXPECT_TRUE(cam2.SaveFrames()); EXPECT_EQ("/tmp", cam2.SaveFramesPath()); EXPECT_EQ("/optical_frame", cam2.OpticalFrameId()); + EXPECT_EQ("/camera_info_test", cam2.CameraInfoTopic()); + EXPECT_EQ("/trigger_topic_test", cam2.TriggerTopic()); + EXPECT_TRUE(cam2.Triggered()); // make changes to DOM and verify ToElement produces updated values cam2.SetNearClip(0.33); diff --git a/src/Converter.cc b/src/Converter.cc index 451f4b696..92dd54ed1 100644 --- a/src/Converter.cc +++ b/src/Converter.cc @@ -30,6 +30,7 @@ #include "Converter.hh" #include "EmbeddedSdf.hh" +#include "Utils.hh" #include "XmlUtils.hh" using namespace sdf; @@ -53,44 +54,62 @@ bool IsNotFlattenedElement(const std::string &_elemName) // used to update //pose/@relative_to in FindNewModelElements() void UpdatePose(tinyxml2::XMLElement *_elem, const size_t &_childNameIdx, - const std::string &_modelName) + const std::string &_modelName, + sdf::Errors &_errors) { tinyxml2::XMLElement *pose = _elem->FirstChildElement("pose"); if (pose && pose->Attribute("relative_to")) { std::string poseRelTo = pose->Attribute("relative_to"); - SDF_ASSERT(poseRelTo.compare(0, _modelName.size(), _modelName) == 0, - "Error: Pose attribute 'relative_to' does not start with " + _modelName); + if (poseRelTo.compare(0, _modelName.size(), _modelName) != 0) + { + std::stringstream ss; + ss << "Error: Pose attribute 'relative_to' does not start with " + << _modelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return; + } poseRelTo = poseRelTo.substr(_childNameIdx); pose->SetAttribute("relative_to", poseRelTo.c_str()); } if (_elem->FirstChildElement("camera")) - UpdatePose(_elem->FirstChildElement("camera"), _childNameIdx, _modelName); + { + UpdatePose(_elem->FirstChildElement("camera"), _childNameIdx, + _modelName, _errors); + } } } ///////////////////////////////////////////////// -bool Converter::Convert(tinyxml2::XMLDocument *_doc, +bool Converter::Convert(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, const std::string &_toVersion, + const ParserConfig &_config, bool _quiet) { - SDF_ASSERT(_doc != nullptr, "SDF XML doc is NULL"); + if (_doc == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF XML doc is NULL"}); + return false; + } tinyxml2::XMLElement *elem = _doc->FirstChildElement("sdf"); // Check that the element exists if (!elem) { - sdferr << " element does not exist.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + " element does not exist."}); return false; } if (!elem->Attribute("version")) { - sdferr << " Unable to determine original SDF version\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Unable to determine original SDF version"}); return false; } @@ -143,18 +162,24 @@ bool Converter::Convert(tinyxml2::XMLDocument *_doc, xmlDoc.Parse(convertXml); if (xmlDoc.Error()) { - sdferr << "Error parsing XML from string: " - << xmlDoc.ErrorStr() << '\n'; + std::stringstream ss; + ss << "Error parsing XML from string: " + << xmlDoc.ErrorStr(); + _errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); return false; } - ConvertImpl(elem, xmlDoc.FirstChildElement("convert")); + ConvertImpl(elem, xmlDoc.FirstChildElement("convert"), _config, _errors); } // Check that we actually converted to the desired final version. if (curVersion != _toVersion) { - sdferr << "Unable to convert from SDF version " << origVersion - << " to " << _toVersion << "\n"; + std::stringstream ss; + ss << "Unable to convert from SDF version " + << origVersion + << " to " + << _toVersion; + _errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); return false; } @@ -162,18 +187,31 @@ bool Converter::Convert(tinyxml2::XMLDocument *_doc, } ///////////////////////////////////////////////// -void Converter::Convert(tinyxml2::XMLDocument *_doc, - tinyxml2::XMLDocument *_convertDoc) +void Converter::Convert(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + tinyxml2::XMLDocument *_convertDoc, + const ParserConfig &_config) { - SDF_ASSERT(_doc != NULL, "SDF XML doc is NULL"); - SDF_ASSERT(_convertDoc != NULL, "Convert XML doc is NULL"); + if (_doc == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF XML doc is NULL"}); + return; + } + if (_convertDoc == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Convert XML doc is NULL"}); + return; + } - ConvertImpl(_doc->FirstChildElement(), _convertDoc->FirstChildElement()); + ConvertImpl(_doc->FirstChildElement(), _convertDoc->FirstChildElement(), + _config, _errors); } ///////////////////////////////////////////////// void Converter::ConvertDescendantsImpl(tinyxml2::XMLElement *_e, - tinyxml2::XMLElement *_c) + tinyxml2::XMLElement *_c, + const ParserConfig &_config, + sdf::Errors &_errors) { if (!_c->Attribute("descendant_name")) { @@ -195,21 +233,31 @@ void Converter::ConvertDescendantsImpl(tinyxml2::XMLElement *_e, { if (strcmp(e->Name(), _c->Attribute("descendant_name")) == 0) { - ConvertImpl(e, _c); + ConvertImpl(e, _c, _config, _errors); } - ConvertDescendantsImpl(e, _c); + ConvertDescendantsImpl(e, _c, _config, _errors); e = e->NextSiblingElement(); } } ///////////////////////////////////////////////// void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_convert) + tinyxml2::XMLElement *_convert, + const ParserConfig &_config, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_convert != NULL, "Convert element is NULL"); + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Convert element is NULL"}); + return; + } - CheckDeprecation(_elem, _convert); + CheckDeprecation(_elem, _convert, _config, _errors); for (auto *convertElem = _convert->FirstChildElement("convert"); convertElem; convertElem = convertElem->NextSiblingElement("convert")) @@ -220,13 +268,13 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, convertElem->Attribute("name")); while (elem) { - ConvertImpl(elem, convertElem); + ConvertImpl(elem, convertElem, _config, _errors); elem = elem->NextSiblingElement(convertElem->Attribute("name")); } } if (convertElem->Attribute("descendant_name")) { - ConvertDescendantsImpl(_elem, convertElem); + ConvertDescendantsImpl(_elem, convertElem, _config, _errors); } } @@ -237,47 +285,55 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, if (name == "rename") { - Rename(_elem, childElem); + Rename(_elem, childElem, _errors); } else if (name == "copy") { - Move(_elem, childElem, true); + Move(_elem, childElem, true, _errors); } else if (name == "map") { - Map(_elem, childElem); + Map(_elem, childElem, _errors); } else if (name == "move") { - Move(_elem, childElem, false); + Move(_elem, childElem, false, _errors); } else if (name == "add") { - Add(_elem, childElem); + Add(_elem, childElem, _errors); } else if (name == "remove") { - Remove(_elem, childElem); + Remove(_errors, _elem, childElem); } else if (name == "remove_empty") { - Remove(_elem, childElem, true); + Remove(_errors, _elem, childElem, true); } else if (name == "unflatten") { - Unflatten(_elem); + Unflatten(_elem, _errors); } else if (name != "convert") { - sdferr << "Unknown convert element[" << name << "]\n"; + std::stringstream ss; + ss << "Unknown convert element[" + << name + << "]"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); } } } ///////////////////////////////////////////////// -void Converter::Unflatten(tinyxml2::XMLElement *_elem) +void Converter::Unflatten(tinyxml2::XMLElement *_elem, sdf::Errors &_errors) { - SDF_ASSERT(_elem != nullptr, "SDF element is nullptr"); + if (_elem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is nullptr"}); + return; + } tinyxml2::XMLDocument *doc = _elem->GetDocument(); @@ -304,7 +360,7 @@ void Converter::Unflatten(tinyxml2::XMLElement *_elem) // recursive unflatten if (elemName == "model") { - Unflatten(elem); + Unflatten(elem, _errors); break; } @@ -315,9 +371,9 @@ void Converter::Unflatten(tinyxml2::XMLElement *_elem) tinyxml2::XMLElement *newModel = doc->NewElement("model"); newModel->SetAttribute("name", newModelName.c_str()); - if (FindNewModelElements(_elem, newModel, found + 2)) + if (FindNewModelElements(_elem, newModel, found + 2, _errors)) { - Unflatten(newModel); + Unflatten(newModel, _errors); _elem->InsertEndChild(newModel); // since newModel is inserted at the end, point back to the top element @@ -332,7 +388,8 @@ void Converter::Unflatten(tinyxml2::XMLElement *_elem) ///////////////////////////////////////////////// bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_newModel, - const size_t &_childNameIdx) + const size_t &_childNameIdx, + sdf::Errors &_errors) { bool unflattenedNewModel = false; std::string newModelName = _newModel->Attribute("name"); @@ -393,9 +450,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, { attachedTo = elem->Attribute("attached_to"); - SDF_ASSERT(attachedTo.compare(0, newModelNameSize, newModelName) == 0, - "Error: Frame attribute 'attached_to' does not start with " + - newModelName); + if (attachedTo.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: Frame attribute 'attached_to' does not start with " + << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } // strip new model prefix from attached_to attachedTo = attachedTo.substr(_childNameIdx); @@ -420,7 +482,7 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, e; e = e->NextSiblingElement()) { - UpdatePose(e, _childNameIdx, newModelName); + UpdatePose(e, _childNameIdx, newModelName, _errors); } } // link @@ -434,8 +496,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, { eText = e->GetText(); - SDF_ASSERT(eText.compare(0, newModelNameSize, newModelName) == 0, - "Error: Joint's value does not start with " + newModelName); + if (eText.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: Joint's value does not start with " + << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } e->SetText(eText.substr(_childNameIdx).c_str()); } @@ -446,8 +514,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, { eText = e->GetText(); - SDF_ASSERT(eText.compare(0, newModelNameSize, newModelName) == 0, - "Error: Joint's value does not start with " + newModelName); + if (eText.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: Joint's value does not start with " + << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } e->SetText(eText.substr(_childNameIdx).c_str()); } @@ -465,10 +539,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, std::string expressIn = axisElem->FirstChildElement("xyz")->Attribute("expressed_in"); - SDF_ASSERT( - expressIn.compare(0, newModelNameSize, newModelName) == 0, - "Error: 's attribute 'expressed_in' does not start with " + - newModelName); + if (expressIn.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: 's attribute 'expressed_in' does not start " + << "with " << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } expressIn = expressIn.substr(_childNameIdx); @@ -487,7 +565,7 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, e; e = e->NextSiblingElement("sensor")) { - UpdatePose(e, _childNameIdx, newModelName); + UpdatePose(e, _childNameIdx, newModelName, _errors); } } // joint @@ -530,9 +608,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, { eText = e->GetText(); - SDF_ASSERT(eText.compare(0, newModelNameSize, newModelName) == 0, - "Error: Gripper's value does not start with " - + newModelName); + if (eText.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: Gripper's value does not start with " + << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } e->SetText(eText.substr(_childNameIdx).c_str()); } @@ -549,10 +632,19 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, ///////////////////////////////////////////////// void Converter::Rename(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_renameElem) + tinyxml2::XMLElement *_renameElem, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_renameElem != NULL, "Rename element is NULL"); + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_renameElem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Rename element is NULL"}); + return; + } auto *fromConvertElem = _renameElem->FirstChildElement("from"); auto *toConvertElem = _renameElem->FirstChildElement("to"); @@ -571,7 +663,8 @@ void Converter::Rename(tinyxml2::XMLElement *_elem, if (!toElemName) { - sdferr << "No 'to' element name specified\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "No 'to' element name specified"}); return; } @@ -601,10 +694,20 @@ void Converter::Rename(tinyxml2::XMLElement *_elem, } ///////////////////////////////////////////////// -void Converter::Add(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_addElem) +void Converter::Add(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_addElem, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_addElem != NULL, "Add element is NULL"); + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_addElem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Add element is NULL"}); + return; + } const char *attributeName = _addElem->Attribute("attribute"); const char *elementName = _addElem->Attribute("element"); @@ -612,8 +715,8 @@ void Converter::Add(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_addElem) if (!((attributeName == nullptr) ^ (elementName == nullptr))) { - sdferr << "Exactly one 'element' or 'attribute'" - << " must be specified in \n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Exactly one 'element' or 'attribute' must be specified in "}); return; } @@ -625,7 +728,8 @@ void Converter::Add(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_addElem) } else { - sdferr << "No 'value' specified in \n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "No 'value' specified in "}); return; } } @@ -643,20 +747,29 @@ void Converter::Add(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_addElem) } ///////////////////////////////////////////////// -void Converter::Remove(tinyxml2::XMLElement *_elem, +void Converter::Remove(sdf::Errors &_errors, + tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_removeElem, bool _removeOnlyEmpty) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_removeElem != NULL, "remove element is NULL"); + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_removeElem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "remove element is NULL"}); + return; + } const char *attributeName = _removeElem->Attribute("attribute"); const char *elementName = _removeElem->Attribute("element"); if (!((attributeName == nullptr) ^ (elementName == nullptr))) { - sdferr << "Exactly one 'element' or 'attribute'" - << " must be specified in \n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Exactly one 'element' or 'attribute' must be specified in "}); return; } @@ -686,22 +799,34 @@ void Converter::Remove(tinyxml2::XMLElement *_elem, } ///////////////////////////////////////////////// -void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) +void Converter::Map(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_mapElem, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != nullptr, "SDF element is nullptr"); - SDF_ASSERT(_mapElem != nullptr, "Map element is nullptr"); + if (_elem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is nullptr"}); + return; + } + if (_mapElem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Map element is nullptr"}); + return; + } tinyxml2::XMLElement *fromConvertElem = _mapElem->FirstChildElement("from"); tinyxml2::XMLElement *toConvertElem = _mapElem->FirstChildElement("to"); if (!fromConvertElem) { - sdferr << " element requires a child element.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + " element requires a child element."}); return; } if (!toConvertElem) { - sdferr << " element requires a child element.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + " element requires a child element."}); return; } @@ -710,12 +835,14 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) if (!fromNameStr || strlen(fromNameStr) == 0) { - sdferr << "Map: element requires a non-empty name attribute.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: element requires a non-empty name attribute."}); return; } if (!toNameStr || strlen(toNameStr) == 0) { - sdferr << "Map: element requires a non-empty name attribute.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: element requires a non-empty name attribute."}); return; } @@ -725,22 +852,26 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) auto *toValueElem = toConvertElem->FirstChildElement("value"); if (!fromValueElem) { - sdferr << "Map: element requires at least one element.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: element requires at least one element"}); return; } if (!toValueElem) { - sdferr << "Map: element requires at least one element.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: element requires at least one element."}); return; } if (!fromValueElem->GetText()) { - sdferr << "Map: from value must not be empty.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: from value must not be empty."}); return; } if (!toValueElem->GetText()) { - sdferr << "Map: to value must not be empty.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: to value must not be empty."}); return; } valueMap[fromValueElem->GetText()] = toValueElem->GetText(); @@ -753,12 +884,14 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) } if (!fromValueElem->GetText()) { - sdferr << "Map: from value must not be empty.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: from value must not be empty."}); return; } if (!toValueElem->GetText()) { - sdferr << "Map: to value must not be empty.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: to value must not be empty."}); return; } valueMap[fromValueElem->GetText()] = toValueElem->GetText(); @@ -791,7 +924,8 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) if (fromLeaf[0] == '\0' || (fromLeaf[0] == '@' && fromLeaf[1] == '\0')) { - sdferr << "Map: has invalid name attribute\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: has invalid name attribute"}); return; } const char *fromValue = nullptr; @@ -834,7 +968,8 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) if (toLeaf[0] == '\0' || (toLeaf[0] == '@' && toLeaf[1] == '\0')) { - sdferr << "Map: has invalid name attribute\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: has invalid name attribute"}); return; } bool toAttribute = toLeaf[0] == '@'; @@ -850,7 +985,8 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) { if (toTokens[newDirIndex].empty()) { - sdferr << "Map: has invalid name attribute\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: has invalid name attribute"}); return; } @@ -875,10 +1011,19 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) ///////////////////////////////////////////////// void Converter::Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy) + const bool _copy, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_moveElem != NULL, "Move element is NULL"); + if (_elem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_moveElem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Move element is NULL"}); + return; + } tinyxml2::XMLElement *fromConvertElem = _moveElem->FirstChildElement("from"); tinyxml2::XMLElement *toConvertElem = _moveElem->FirstChildElement("to"); @@ -1061,7 +1206,9 @@ const char *Converter::GetValue(const char *_valueElem, const char *_valueAttr, ///////////////////////////////////////////////// void Converter::CheckDeprecation(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_convert) + tinyxml2::XMLElement *_convert, + const ParserConfig &_config, + sdf::Errors &_errors) { // Process deprecated elements for (auto *deprecatedElem = _convert->FirstChildElement("deprecated"); @@ -1097,7 +1244,11 @@ void Converter::CheckDeprecation(tinyxml2::XMLElement *_elem, } } - sdfwarn << "Deprecated SDF Values in original file:\n" - << stream.str() << "\n\n"; + std::stringstream ss; + ss << "Deprecated SDF Values in original file: \n" + << stream.str() << "\n"; + Error err(ErrorCode::WARNING, ss.str()); + sdf::enforceConfigurablePolicyCondition(_config.WarningsPolicy(), + err, _errors); } } diff --git a/src/Converter.hh b/src/Converter.hh index 5c3badb17..cfba5413f 100644 --- a/src/Converter.hh +++ b/src/Converter.hh @@ -23,6 +23,7 @@ #include #include +#include #include "sdf/system_util.hh" namespace sdf @@ -35,51 +36,71 @@ namespace sdf class Converter { /// \brief Convert SDF to the specified version. + /// \param[out] _errors Vector of errors. /// \param[in] _doc SDF xml doc - /// \param[in] _toVersion Version number in string format - /// \param[in] _quiet False to be more verbose - public: static bool Convert(tinyxml2::XMLDocument *_doc, + /// \param[in] _toVersion Version number in string format. + /// \param[in] _config Parser configuration. + /// \param[in] _quiet False to be more verbose. + public: static bool Convert(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, const std::string &_toVersion, + const ParserConfig &_config, bool _quiet = false); /// \cond /// This is an internal function. /// \brief Generic convert function that converts the SDF based on the /// given Convert file. + /// \param[out] _errors Vector of errors. /// \param[in] _doc SDF xml doc /// \param[in] _convertDoc Convert xml doc - public: static void Convert(tinyxml2::XMLDocument *_doc, - tinyxml2::XMLDocument *_convertDoc); + /// \param[in] _config Parser configuration. + public: static void Convert(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + tinyxml2::XMLDocument *_convertDoc, + const ParserConfig &_config); /// \endcond /// \brief Implementation of Convert functionality. /// \param[in] _elem SDF xml element tree to convert. /// \param[in] _convert Convert xml element tree. + /// \param[in] _config Parser configuration. + /// \param[out] _errors Vector of errors. private: static void ConvertImpl(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_convert); + tinyxml2::XMLElement *_convert, + const ParserConfig &_config, + sdf::Errors &_errors); /// \brief Recursive helper function for ConvertImpl that converts /// elements named by the descendant_name attribute. /// \param[in] _e SDF xml element tree to convert. /// \param[in] _c Convert xml element tree. + /// \param[in] _config Parser configuration. + /// \param[out] _errors Vector of errors. private: static void ConvertDescendantsImpl(tinyxml2::XMLElement *_e, - tinyxml2::XMLElement *_c); + tinyxml2::XMLElement *_c, + const ParserConfig &_config, + sdf::Errors &_errors); /// \brief Rename an element or attribute. /// \param[in] _elem The element to be renamed, or the element which /// has the attribute to be renamed. - /// \param[in] _renameElem A 'convert' element that describes the rename + /// \param[in] _renameElem A 'convert' element that describes the rename. + /// \param[out] _errors Vector of errors. /// operation. private: static void Rename(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_renameElem); + tinyxml2::XMLElement *_renameElem, + sdf::Errors &_errors); /// \brief Map values from one element or attribute to another. /// \param[in] _elem Ancestor element of the element or attribute to /// be mapped. /// \param[in] _mapElem A 'convert' element that describes the map + /// \param[out] _errors Vector of errors. /// operation. private: static void Map(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_mapElem); + tinyxml2::XMLElement *_mapElem, + sdf::Errors &_errors); /// \brief Move an element or attribute within a common ancestor element. /// \param[in] _elem Ancestor element of the element or attribute to @@ -87,47 +108,59 @@ namespace sdf /// \param[in] _moveElem A 'convert' element that describes the move /// operation. /// \param[in] _copy True to copy the element + /// \param[out] _errors Vector of errors. private: static void Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy); + const bool _copy, + sdf::Errors &_errors); /// \brief Add an element or attribute to an element. /// \param[in] _elem The element to receive the value. /// \param[in] _addElem A 'convert' element that describes the add + /// \param[out] _errors Vector of Errors. /// operation. private: static void Add(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_addElem); + tinyxml2::XMLElement *_addElem, + sdf::Errors &_errors); /// \brief Remove an attribute or elements. + /// \param[out] _errors Vector of Errors. /// \param[in] _elem The element from which data may be removed. /// \param[in] _removeElem The metadata about what to remove. /// \param[in] _removeOnlyEmpty If true, only remove an attribute /// containing an empty string or elements that contain neither value nor /// child elements nor attributes. - private: static void Remove(tinyxml2::XMLElement *_elem, + private: static void Remove(sdf::Errors &_errors, + tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_removeElem, bool _removeOnlyEmpty = false); /// \brief Unflatten an element (conversion from SDFormat <= 1.7 to 1.8) /// \param[in] _elem The element to unflatten - private: static void Unflatten(tinyxml2::XMLElement *_elem); + /// \param[out] _errors Vector of errors + private: static void Unflatten(tinyxml2::XMLElement *_elem, + sdf::Errors &_errors); /// \brief Finds all elements related to the unflattened model /// \param[in] _elem The element to unflatten /// \param[in] _newModel The new unflattened model element /// \param[in] _childNameIdx The beginning index of child element names + /// \param[out] _errors Vector of errors /// (e.g., in newModelName::childName then _childNameIdx = 14) /// \return True if unflattened new model elements private: static bool FindNewModelElements(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_newModel, - const size_t &_childNameIdx); + const size_t &_childNameIdx, + sdf::Errors &_errors); private: static const char *GetValue(const char *_valueElem, const char *_valueAttr, tinyxml2::XMLElement *_elem); private: static void CheckDeprecation(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_convert); + tinyxml2::XMLElement *_convert, + const ParserConfig &_config, + sdf::Errors &_errors); }; } } diff --git a/src/Converter_TEST.cc b/src/Converter_TEST.cc index 73cb54b05..679e31d2e 100644 --- a/src/Converter_TEST.cc +++ b/src/Converter_TEST.cc @@ -20,8 +20,10 @@ #include #include "sdf/Exception.hh" #include "sdf/Filesystem.hh" +#include "sdf/ParserConfig.hh" #include "Converter.hh" +#include "test_utils.hh" #include "XmlUtils.hh" #include "test_config.hh" @@ -98,7 +100,27 @@ TEST(Converter, MoveElemElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -136,7 +158,27 @@ TEST(Converter, MoveElemAttr) << ""; tinyxml2::XMLDocument convertXmlDoc2; convertXmlDoc2.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc2, &convertXmlDoc2); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc2, &convertXmlDoc2, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc2.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -174,7 +216,27 @@ TEST(Converter, MoveAttrAttr) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -215,7 +277,27 @@ TEST(Converter, MoveAttrElem) << ""; tinyxml2::XMLDocument convertXmlDoc4; convertXmlDoc4.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc4, &convertXmlDoc4); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc4, &convertXmlDoc4, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc4.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -254,7 +336,27 @@ TEST(Converter, MoveElemElemMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc5; convertXmlDoc5.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc5, &convertXmlDoc5); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc5, &convertXmlDoc5, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc5.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -289,7 +391,27 @@ TEST(Converter, MoveAttrAttrMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc6; convertXmlDoc6.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc6, &convertXmlDoc6); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc6, &convertXmlDoc6, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc6.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -327,7 +449,27 @@ TEST(Converter, MoveElemAttrMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc7; convertXmlDoc7.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc7, &convertXmlDoc7); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc7, &convertXmlDoc7, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc7.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -362,7 +504,27 @@ TEST(Converter, MoveAttrElemMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc8; convertXmlDoc8.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc8, &convertXmlDoc8); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc8, &convertXmlDoc8, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc8.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -420,7 +582,27 @@ TEST(Converter, Add) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -463,7 +645,30 @@ TEST(Converter, AddNoElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Exactly one 'element' or 'attribute' must be specified in ")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Verify the xml tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); @@ -505,7 +710,30 @@ TEST(Converter, AddNoValue) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("No 'value' specified in ")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); @@ -557,7 +785,27 @@ TEST(Converter, RemoveElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -606,7 +854,27 @@ TEST(Converter, RemoveDescendantElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -658,7 +926,27 @@ TEST(Converter, RemoveEmptyElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -722,7 +1010,27 @@ TEST(Converter, RemoveEmptyDescendantElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -787,7 +1095,28 @@ TEST(Converter, RemoveDescendantNestedElement) tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertString.c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); std::string expectedString = R"( @@ -846,7 +1175,27 @@ TEST(Converter, DescendantIgnorePluginOrNamespacedElements) tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertString.c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Only the elemB directly under the first elemA should be removed std::string expectedString = R"( @@ -909,7 +1258,27 @@ TEST(Converter, RemoveElementSubElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -955,7 +1324,27 @@ TEST(Converter, RemoveAttr) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1007,7 +1396,27 @@ TEST(Converter, RemoveEmptyAttr) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1045,7 +1454,30 @@ TEST(Converter, RemoveNoElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Exactly one 'element' or 'attribute' must be specified in ")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); @@ -1097,7 +1529,27 @@ TEST(Converter, MoveInvalid) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // In this case, we had an invalid elemC:: in the conversion, which // means that the conversion quietly failed. Make sure the new @@ -1153,7 +1605,27 @@ TEST(Converter, MoveInvalidPrefix) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // In this case, we had an invalid ::elemC in the conversion, which // means that the conversion quietly failed. Make sure the new @@ -1210,7 +1682,27 @@ TEST(Converter, CopyElemElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1385,7 +1877,75 @@ TEST(Converter, MapInvalid) tinyxml2::XMLPrinter printerBefore; xmlDoc.Print(&printerBefore); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 16u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[1].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[2].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[3].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[4].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[5].Message().find( + " element requires a child element.")); + EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[6].Message().find( + " element requires a child element.")); + EXPECT_EQ(errors[7].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[7].Message().find( + " element requires a non-empty name attribute.")); + EXPECT_EQ(errors[8].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[8].Message().find( + " element requires a non-empty name attribute.")); + EXPECT_EQ(errors[9].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[9].Message().find( + " element requires a non-empty name attribute.")); + EXPECT_EQ(errors[10].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[10].Message().find( + " element requires at least one element")); + EXPECT_EQ(errors[11].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[11].Message().find( + " element requires at least one element")); + EXPECT_EQ(errors[12].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[12].Message().find("from value must not be empty.")); + EXPECT_EQ(errors[13].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[13].Message().find("to value must not be empty.")); + EXPECT_EQ(errors[14].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[14].Message().find("from value must not be empty.")); + EXPECT_EQ(errors[15].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[15].Message().find("to value must not be empty.")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + // Only invalid conversion statements. // Make sure the new document is the same as the original. @@ -1495,7 +2055,27 @@ TEST(Converter, MapElemElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1546,7 +2126,27 @@ TEST(Converter, MapElemAttr) << ""; tinyxml2::XMLDocument convertXmlDoc2; convertXmlDoc2.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc2, &convertXmlDoc2); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc2, &convertXmlDoc2, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc2.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1589,7 +2189,27 @@ TEST(Converter, MapAttrAttr) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1639,7 +2259,27 @@ TEST(Converter, MapAttrElem) << ""; tinyxml2::XMLDocument convertXmlDoc4; convertXmlDoc4.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc4, &convertXmlDoc4); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc4, &convertXmlDoc4, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc4.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1686,7 +2326,27 @@ TEST(Converter, MapElemElemMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc5; convertXmlDoc5.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc5, &convertXmlDoc5); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc5, &convertXmlDoc5, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc5.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1730,7 +2390,27 @@ TEST(Converter, MapAttrAttrMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc6; convertXmlDoc6.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc6, &convertXmlDoc6); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc6, &convertXmlDoc6, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc6.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -1777,7 +2457,27 @@ TEST(Converter, MapElemAttrMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc7; convertXmlDoc7.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc7, &convertXmlDoc7); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc7, &convertXmlDoc7, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc7.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -1821,7 +2521,27 @@ TEST(Converter, MapAttrElemMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc8; convertXmlDoc8.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc8, &convertXmlDoc8); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc8, &convertXmlDoc8, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc8.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -1882,7 +2602,27 @@ TEST(Converter, RenameElemElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1920,7 +2660,27 @@ TEST(Converter, RenameAttrAttr) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1957,7 +2717,27 @@ TEST(Converter, RenameNoFrom) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1994,7 +2774,30 @@ TEST(Converter, RenameNoTo) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("No 'to' element name specified")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -2019,7 +2822,28 @@ TEST(Converter, GazeboToSDF) tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - EXPECT_FALSE(sdf::Converter::Convert(&xmlDoc, "1.3")); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + EXPECT_FALSE(sdf::Converter::Convert(errors, &xmlDoc, "1.3", parserConfig)); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find(" element does not exist.")); } //////////////////////////////////////////////////// @@ -2028,12 +2852,44 @@ TEST(Converter, NullDoc) tinyxml2::XMLDocument xmlDoc; tinyxml2::XMLDocument convertXmlDoc; - ASSERT_THROW(sdf::Converter::Convert(nullptr, &convertXmlDoc), - sdf::AssertionInternalError); - ASSERT_THROW(sdf::Converter::Convert(&xmlDoc, nullptr), - sdf::AssertionInternalError); - ASSERT_THROW(sdf::Converter::Convert(nullptr, "1.4"), - sdf::AssertionInternalError); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + sdf::Converter::Convert(errors, nullptr, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FATAL_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("SDF XML doc is NULL")); + errors.clear(); + + sdf::Converter::Convert(errors, &xmlDoc, nullptr, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FATAL_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("Convert XML doc is NULL")); + errors.clear(); + + sdf::Converter::Convert(errors, nullptr, "1.4", parserConfig); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FATAL_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("SDF XML doc is NULL")); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } //////////////////////////////////////////////////// @@ -2044,7 +2900,30 @@ TEST(Converter, NoVersion) tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - ASSERT_FALSE(sdf::Converter::Convert(&xmlDoc, "1.3")); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + ASSERT_FALSE(sdf::Converter::Convert(errors, &xmlDoc, "1.3", parserConfig)); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Unable to determine original SDF version")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } //////////////////////////////////////////////////// @@ -2058,7 +2937,27 @@ TEST(Converter, SameVersion) tinyxml2::XMLPrinter printerBefore; xmlDoc.Print(&printerBefore); - ASSERT_TRUE(sdf::Converter::Convert(&xmlDoc, "1.3")); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + ASSERT_TRUE(sdf::Converter::Convert(errors, &xmlDoc, "1.3", parserConfig)); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLPrinter printerAfter; xmlDoc.Print(&printerAfter); @@ -2075,7 +2974,27 @@ TEST(Converter, NewerVersion) tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - ASSERT_TRUE(sdf::Converter::Convert(&xmlDoc, "1.6")); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + ASSERT_TRUE(sdf::Converter::Convert(errors, &xmlDoc, "1.6", parserConfig)); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } //////////////////////////////////////////////////// @@ -2086,7 +3005,27 @@ TEST(Converter, MuchNewerVersion) tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - ASSERT_TRUE(sdf::Converter::Convert(&xmlDoc, "1.6")); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + ASSERT_TRUE(sdf::Converter::Convert(errors, &xmlDoc, "1.6", parserConfig)); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } static std::string ConvertDoc_15_16() @@ -2139,7 +3078,27 @@ TEST(Converter, IMU_15_to_16) // Convert tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.LoadFile(ConvertDoc_15_16().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Check some basic elements tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); @@ -2233,7 +3192,27 @@ TEST(Converter, World_15_to_16) // Convert tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.LoadFile(ConvertDoc_15_16().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Check some basic elements tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); @@ -2289,7 +3268,27 @@ TEST(Converter, Pose_16_to_17) // Convert tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.LoadFile(ConvertDoc_16_17().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Check some basic elements tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); @@ -2374,7 +3373,26 @@ TEST(Converter, World_17_to_18) // Convert tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.LoadFile(ConvertDoc_17_18().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); // Compare converted xml with expected std::string convertedXmlStr = ElementToString(xmlDoc.RootElement()); @@ -2512,7 +3530,8 @@ TEST(Converter, World_17_to_18) xmlDoc.Clear(); xmlDoc.Parse(xmlString.c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); // Compare converted xml with expected convertedXmlStr = ElementToString(xmlDoc.RootElement()); @@ -2614,7 +3633,12 @@ TEST(Converter, World_17_to_18) xmlDoc.Clear(); xmlDoc.Parse(xmlString.c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); + + // Check nothing has been printed during Convert calls + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Compare converted xml with expected convertedXmlStr = ElementToString(xmlDoc.RootElement()); diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 0f22e111b..e4d0aedad 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -599,6 +599,10 @@ struct WorldWrapper : public WrapperBase { this->frames.emplace_back(*_world.FrameByIndex(i)); } + for (uint64_t i = 0; i < _world.JointCount(); ++i) + { + this->joints.emplace_back(*_world.JointByIndex(i)); + } for (uint64_t i = 0; i < _world.ModelCount(); ++i) { this->models.emplace_back(*_world.ModelByIndex(i)); @@ -612,6 +616,8 @@ struct WorldWrapper : public WrapperBase /// \brief Children frames. std::vector frames; + /// \brief Children joints. + std::vector joints; /// \brief Children models and interface models. std::vector models; }; @@ -755,11 +761,11 @@ void addEdgesToGraph(ScopedGraph &_out, /// \param[in,out] _out The FrameAttachedTo graph to which edges will be added. /// \param[in] _joints List of joints for which edges will be created in the /// graph. -/// \param[in] _model Parent model of the items in `_items`. +/// \param[in] _parent Parent of the joints in `_joints`. /// \param[out] _errors Errors encountered while adding edges. void addEdgesToGraph(ScopedGraph &_out, const std::vector &_joints, - const ModelWrapper &_model, Errors &_errors) + const WrapperBase &_parent, Errors &_errors) { for (const auto &joint : _joints) { @@ -771,7 +777,8 @@ void addEdgesToGraph(ScopedGraph &_out, {ErrorCode::JOINT_CHILD_LINK_INVALID, "Child frame with name[" + joint.childName + "] specified by " + lowercase(joint.elementType) + " with name[" + joint.name + - "] not found in model with name[" + _model.name + "]."}); + "] not found in " + lowercase(_parent.elementType) + + " with name[" + _parent.name + "]."}); continue; } auto childFrameId = _out.VertexIdByName(joint.childName); @@ -784,7 +791,7 @@ void addEdgesToGraph(ScopedGraph &_out, /// \param[in,out] _out The FrameAttachedTo graph to which edges will be added. /// \param[in] _frames List of frames for which edges will be created in the /// graph. -/// \param[in] _model Parent model of the items in `_items`. +/// \param[in] _parent Parent of the frames in `_frames`. /// \param[out] _errors Errors encountered while adding edges. void addEdgesToGraph(ScopedGraph &_out, const std::vector &_frames, @@ -991,9 +998,15 @@ Errors buildFrameAttachedToGraph( // add model vertices addVerticesToGraph(_out, _world.models, _world, errors); + // add joint vertices + addVerticesToGraph(_out, _world.joints, _world, errors); + // add frame vertices addVerticesToGraph(_out, _world.frames, _world, errors); + // add edges from joint to child frames + addEdgesToGraph(_out, _world.joints, _world, errors); + // add frame edges addEdgesToGraph(_out, _world.frames, _world, errors); @@ -1161,6 +1174,9 @@ Errors wrapperBuildPoseRelativeToGraph( // add model vertices addVerticesToGraph(_out, _world.models, _world, errors); + // add joint vertices + addVerticesToGraph(_out, _world.joints, _world, errors); + // add frame vertices addVerticesToGraph(_out, _world.frames, _world, errors); @@ -1169,6 +1185,9 @@ Errors wrapperBuildPoseRelativeToGraph( // add model edges addEdgesToGraph(_out, _world.models, _world, errors); + // add joint edges + addEdgesToGraph(_out, _world.joints, _world, errors); + // add frame edges addEdgesToGraph(_out, _world.frames, _world, errors); diff --git a/src/Joint.cc b/src/Joint.cc index d4574a308..f27405c86 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -52,7 +52,8 @@ class sdf::Joint::Implementation /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; - /// \brief Thread pitch for screw joints. + /// \brief Thread pitch for screw joints in meters / revolution with a + /// positive value for right-handed threads. public: double threadPitch = 1.0; /// \brief Joint axis @@ -179,7 +180,18 @@ Errors Joint::Load(ElementPtr _sdf) errors.insert(errors.end(), axisErrors.begin(), axisErrors.end()); } - this->dataPtr->threadPitch = _sdf->Get("thread_pitch", 1.0).first; + if (_sdf->HasElement("screw_thread_pitch")) + { + // Prefer the screw_thread_pitch parameter if available. + this->dataPtr->threadPitch = _sdf->Get("screw_thread_pitch"); + } + else if (_sdf->HasElement("thread_pitch")) + { + // If thread_pitch is available, convert to meters / revolution + // and fix sign. + this->dataPtr->threadPitch = -2*GZ_PI / _sdf->Get("thread_pitch"); + } + // Otherwise the default value of threadPitch will be used // Read the type std::pair typePair = _sdf->Get("type", ""); @@ -426,17 +438,29 @@ sdf::SemanticPose Joint::SemanticPose() const } ///////////////////////////////////////////////// -double Joint::ThreadPitch() const +double Joint::ScrewThreadPitch() const { return this->dataPtr->threadPitch; } ///////////////////////////////////////////////// -void Joint::SetThreadPitch(double _threadPitch) +void Joint::SetScrewThreadPitch(double _threadPitch) { this->dataPtr->threadPitch = _threadPitch; } +///////////////////////////////////////////////// +double Joint::ThreadPitch() const +{ + return -2*GZ_PI / this->dataPtr->threadPitch; +} + +///////////////////////////////////////////////// +void Joint::SetThreadPitch(double _threadPitch) +{ + this->dataPtr->threadPitch = -2*GZ_PI / _threadPitch; +} + ///////////////////////////////////////////////// sdf::ElementPtr Joint::Element() const { diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index 50d4682e6..f4c23abf9 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -119,10 +119,18 @@ TEST(DOMJoint, Construction) EXPECT_EQ(axis.Xyz(), joint.Axis(0)->Xyz()); EXPECT_EQ(axis1.Xyz(), joint.Axis(1)->Xyz()); - EXPECT_DOUBLE_EQ(1.0, joint.ThreadPitch()); + // Default thread pitch + EXPECT_DOUBLE_EQ(1.0, joint.ScrewThreadPitch()); + EXPECT_DOUBLE_EQ(-2*GZ_PI, joint.ThreadPitch()); + + // Set and check thread pitch const double threadPitch = 0.1; + joint.SetScrewThreadPitch(threadPitch); + EXPECT_DOUBLE_EQ(threadPitch, joint.ScrewThreadPitch()); + EXPECT_DOUBLE_EQ(-2*GZ_PI / threadPitch, joint.ThreadPitch()); joint.SetThreadPitch(threadPitch); EXPECT_DOUBLE_EQ(threadPitch, joint.ThreadPitch()); + EXPECT_DOUBLE_EQ(-2*GZ_PI / threadPitch, joint.ScrewThreadPitch()); EXPECT_EQ(0u, joint.SensorCount()); EXPECT_EQ(nullptr, joint.SensorByIndex(0)); diff --git a/src/Model.cc b/src/Model.cc index ae7840973..4f4f7b0f3 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -203,9 +203,13 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { if (size > 1) { - sdfwarn << "Non-unique name[" << name << "] detected " << size - << " times in XML children of model with name[" << this->Name() - << "].\n"; + std::stringstream ss; + ss << "Non-unique name[" << name << "] detected " << size + << " times in XML children of model with name[" << this->Name() + << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); } } @@ -298,17 +302,23 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { linkName = link.Name() + "_link" + std::to_string(i++); } - sdfwarn << "Link with name [" << link.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision, changing link name to [" - << linkName << "].\n"; + std::stringstream ss; + ss << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing link name to [" + << linkName << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); link.SetName(linkName); } else { - sdferr << "Link with name [" << link.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision. Please rename this link.\n"; + std::stringstream ss; + ss << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this link."; + errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); } } frameNames.insert(linkName); @@ -346,17 +356,24 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { jointName = joint.Name() + "_joint" + std::to_string(i++); } - sdfwarn << "Joint with name [" << joint.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision, changing joint name to [" - << jointName << "].\n"; + std::stringstream ss; + ss << "Joint with name [" << joint.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing joint name to [" + << jointName << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); + joint.SetName(jointName); } else { - sdferr << "Joint with name [" << joint.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision. Please rename this joint.\n"; + std::stringstream ss; + ss << "Joint with name [" << joint.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this joint."; + errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); } } frameNames.insert(jointName); @@ -383,17 +400,24 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { frameName = frame.Name() + "_frame" + std::to_string(i++); } - sdfwarn << "Frame with name [" << frame.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision, changing frame name to [" - << frameName << "].\n"; + std::stringstream ss; + ss << "Frame with name [" << frame.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing frame name to [" + << frameName << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); + frame.SetName(frameName); } else { - sdferr << "Frame with name [" << frame.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision. Please rename this frame.\n"; + std::stringstream ss; + ss << "Frame with name [" << frame.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this frame."; + errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); } } frameNames.insert(frameName); diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 0da219e48..14e26bf5c 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -154,6 +154,10 @@ TEST(DOMModel, Construction) errors[1].Message().find( "PoseRelativeToGraph error: scope does not point to a valid graph")) << errors[1]; + + // model doesn't have graphs, so no names should exist in graphs + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("link")); } ///////////////////////////////////////////////// @@ -229,6 +233,9 @@ TEST(DOMModel, AddLink) EXPECT_EQ(1u, model.LinkCount()); EXPECT_FALSE(model.AddLink(link)); EXPECT_EQ(1u, model.LinkCount()); + // model doesn't have graphs, so no names should exist in graphs + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("link1")); model.ClearLinks(); EXPECT_EQ(0u, model.LinkCount()); diff --git a/src/Param.cc b/src/Param.cc index 8d0ce5a30..2336f4296 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -1473,7 +1473,7 @@ bool Param::ValidateValue(sdf::Errors &_errors) const std::ostringstream oss; oss << "The value [" << _val << "] is less than the minimum allowed value of [" - << *this->GetMinValueAsString() << "] for key [" + << *this->GetMinValueAsString(_errors) << "] for key [" << this->GetKey() << "]"; _errors.push_back({ErrorCode::PARAMETER_ERROR, oss.str()}); return false; @@ -1486,7 +1486,7 @@ bool Param::ValidateValue(sdf::Errors &_errors) const std::ostringstream oss; oss << "The value [" << _val << "] is greater than the maximum allowed value of [" - << *this->GetMaxValueAsString() << "] for key [" + << *this->GetMaxValueAsString(_errors) << "] for key [" << this->GetKey() << "]"; _errors.push_back({ErrorCode::PARAMETER_ERROR, oss.str()}); return false; diff --git a/src/ParamPassing.cc b/src/ParamPassing.cc index cea6e8b77..1b80d09cf 100644 --- a/src/ParamPassing.cc +++ b/src/ParamPassing.cc @@ -24,6 +24,7 @@ #include "ParamPassing.hh" #include "parser_private.hh" +#include "Utils.hh" #include "XmlUtils.hh" namespace sdf @@ -176,11 +177,11 @@ void updateParams(const ParserConfig &_config, } else if (actionStr == "modify") { - modify(childElemXml, elem, _errors); + modify(childElemXml, _config, elem, _errors); } else if (actionStr == "remove") { - remove(childElemXml, elem, _errors); + remove(childElemXml, _config, elem, _errors); } else if (actionStr == "replace") { @@ -292,6 +293,8 @@ bool isValidAction(const std::string &_action) ////////////////////////////////////////////////// ElementPtr getElementByName(const ElementPtr _elem, const tinyxml2::XMLElement *_xml, + const sdf::ParserConfig _config, + sdf::Errors &_errors, const bool _isModifyAction) { std::string elemName = _xml->Name(); @@ -324,10 +327,14 @@ ElementPtr getElementByName(const ElementPtr _elem, else if (elem->HasAttribute("name") && elem->GetAttribute("name")->GetRequired()) { - sdfwarn << "The original element [" << elemName << "] contains the " - << "attribute 'name' but none was provided in the element modifier." - << " The assumed element to be modified is: <" << elemName - << " name='" << elem->Get("name") << "'>\n"; + std::stringstream ss; + ss << "The original element [" << elemName << "] contains the " + << "attribute 'name' but none was provided in the element modifier." + << " The assumed element to be modified is: <" << elemName + << " name='" << elem->Get("name") << "'>"; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, _errors); } return elem; @@ -397,7 +404,7 @@ void handleIndividualChildActions(const ParserConfig &_config, if (actionStr == "remove") { - ElementPtr e = getElementByName(_elem, xmlChild); + ElementPtr e = getElementByName(_elem, xmlChild, _config, _errors); if (e == nullptr) { _errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -409,14 +416,14 @@ void handleIndividualChildActions(const ParserConfig &_config, } else { - remove(xmlChild, e, _errors); + remove(xmlChild, _config, e, _errors); } continue; } else if (actionStr == "modify") { - ElementPtr e = getElementByName(_elem, xmlChild, true); + ElementPtr e = getElementByName(_elem, xmlChild, _config, _errors, true); if (e == nullptr) { _errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -428,7 +435,7 @@ void handleIndividualChildActions(const ParserConfig &_config, } else { - modify(xmlChild, e, _errors); + modify(xmlChild, _config, e, _errors); } continue; @@ -472,7 +479,7 @@ void handleIndividualChildActions(const ParserConfig &_config, } else if (actionStr == "replace") { - ElementPtr e = getElementByName(_elem, xmlChild); + ElementPtr e = getElementByName(_elem, xmlChild, _config, _errors); if (e == nullptr) { _errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -562,7 +569,8 @@ void modifyAttributes(tinyxml2::XMLElement *_xml, ////////////////////////////////////////////////// void modifyChildren(tinyxml2::XMLElement *_xml, - ElementPtr _elem, Errors &_errors) + const sdf::ParserConfig &_config, ElementPtr _elem, + Errors &_errors) { for (tinyxml2::XMLElement *xmlChild = _xml->FirstChildElement(); xmlChild; @@ -579,7 +587,8 @@ void modifyChildren(tinyxml2::XMLElement *_xml, continue; } - ElementPtr elemChild = getElementByName(_elem, xmlChild, true); + ElementPtr elemChild = getElementByName(_elem, xmlChild, + _config, _errors, true); ParamPtr paramChild = elemChild->GetValue(); if (xmlChild->GetText()) @@ -609,21 +618,26 @@ void modifyChildren(tinyxml2::XMLElement *_xml, else { // sdf has child elements but no children were specified in xml - sdfwarn << "No modifications for element " - << ElementToString(xmlChild) - << " provided, skipping modification for:\n" - << ElementToString(_xml) << "\n"; + std::stringstream ss; + ss << "No modifications for element " + << ElementToString(xmlChild) + << " provided, skipping modification for:\n" + << ElementToString(_xml); + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, _errors); } } else { - modify(xmlChild, elemChild, _errors); + modify(xmlChild, _config, elemChild, _errors); } } } ////////////////////////////////////////////////// -void modify(tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors) +void modify(tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, + ElementPtr _elem, Errors &_errors) { modifyAttributes(_xml, _elem, _errors); @@ -643,12 +657,13 @@ void modify(tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors) else { // modify children elements - modifyChildren(_xml, _elem, _errors); + modifyChildren(_xml, _config, _elem, _errors); } } ////////////////////////////////////////////////// -void remove(const tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors) +void remove(const tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, + ElementPtr _elem, Errors &_errors) { if (_xml->NoChildren()) { @@ -663,7 +678,7 @@ void remove(const tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors) xmlChild; xmlChild = xmlChild->NextSiblingElement()) { - elemChild = getElementByName(_elem, xmlChild); + elemChild = getElementByName(_elem, xmlChild, _config, _errors); if (elemChild == nullptr) { const tinyxml2::XMLElement *xmlParent = _xml->Parent()->ToElement(); diff --git a/src/ParamPassing.hh b/src/ParamPassing.hh index 1572f6dda..ac077debb 100644 --- a/src/ParamPassing.hh +++ b/src/ParamPassing.hh @@ -85,6 +85,8 @@ namespace sdf /// \param[in] _elem The sdf (parent) element used to find the matching /// child element described in xml /// \param[in] _xml The xml element to find + /// \param[in] _config Custom parser configuration. + /// \param[out] _errors Vector of errros. /// \param[in] _isModifyAction Is true if the action is modify, the /// attribute 'name' may not be in the sdf element (i.e., may be a /// modified/added attribute such as //camera) @@ -92,6 +94,8 @@ namespace sdf /// element could not be found ElementPtr getElementByName(const ElementPtr _elem, const tinyxml2::XMLElement *_xml, + const sdf::ParserConfig _config, + sdf::Errors &_errors, const bool _isModifyAction = false); /// \brief Initialize an sdf element description from the xml element @@ -143,24 +147,31 @@ namespace sdf /// \brief Modifies the children elements of the included model /// \param[in] _xml Pointer to the xml element which contains the elements /// to be modified + /// \param[in] _config Custom parser configuration. /// \param[out] _elem The element from the included model to modify + /// \param[out] _errors Vector of errors. void modifyChildren(tinyxml2::XMLElement *_xml, - ElementPtr _elem, Errors &_errors); + const sdf::ParserConfig &_config, ElementPtr _elem, + Errors &_errors); /// \brief Modifies element values and/or attributes of an element from the /// included model /// \param[in] _xml Pointer to the xml element which contains the elements /// to be modified + /// \param[in] _config Custom parser configuration. /// \param[out] _elem The element from the included model to modify /// \param[out] _errors Captures errors found during parsing - void modify(tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors); + void modify(tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, + ElementPtr _elem, Errors &_errors); /// \brief Removes an element specified in xml /// \param[in] _xml Pointer to the xml element(s) to be removed from _elem + /// \param[in] _config Custom parser configuration. /// \param[out] _elem The element from the included model to remove /// elements from /// \param[out] _errors Captures errors found during parsing - void remove(const tinyxml2::XMLElement *_xml, ElementPtr _elem, + void remove(const tinyxml2::XMLElement *_xml, + const sdf::ParserConfig &_config, ElementPtr _elem, Errors &_errors); /// \brief Replace an element with another element diff --git a/src/ParamPassing_TEST.cc b/src/ParamPassing_TEST.cc index f9c9a5704..00f9ff48a 100644 --- a/src/ParamPassing_TEST.cc +++ b/src/ParamPassing_TEST.cc @@ -20,6 +20,7 @@ #include "ParamPassing.hh" #include "sdf/Element.hh" #include "sdf/parser.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// TEST(ParamPassing, GetElement) @@ -91,3 +92,125 @@ TEST(ParamPassing, GetElement) "model::test_link::test_visual"); EXPECT_EQ(nullptr, paramPassElem); } + +//////////////////////////////////////// +// Test warnings outputs for GetElementByName +TEST(ParamPassing, GetElementByNameWarningOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + std::ostringstream stream; + stream << "" + << "" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + + sdf::Errors errors; + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::readString(stream.str(), parserConfig, sdfParsed, errors); + EXPECT_TRUE(errors.empty()); + + std::ostringstream stream2; + stream2 << " " + << " "; + tinyxml2::XMLDocument doc; + doc.Parse(stream2.str().c_str()); + + sdf::ParamPassing::getElementByName(sdfParsed->Root(), + doc.FirstChildElement("model"), + parserConfig, + errors); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "The original element [model] contains the attribute 'name' but none was" + " provided in the element modifier. The assumed element to be modified " + "is: ")); + errors.clear(); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::WARN); + sdf::ParamPassing::getElementByName(sdfParsed->Root(), + doc.FirstChildElement("model"), + parserConfig, + errors); + EXPECT_TRUE(errors.empty()); + // Check the warning has been printed + EXPECT_NE(std::string::npos, buffer.str().find( + "The original element [model] contains the attribute 'name' but none " + "was provided in the element modifier. The assumed element to be " + "modified is: ")); +} + +//////////////////////////////////////// +// Test warnings outputs for GetElementByName +TEST(ParamPassing, ModifyChildrenNameWarningOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + std::ostringstream stream; + stream << "" + << "" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + + sdf::Errors errors; + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::readString(stream.str(), parserConfig, sdfParsed, errors); + EXPECT_TRUE(errors.empty()); + + std::ostringstream stream2; + stream2 << "" + << " " + << " " + << ""; + tinyxml2::XMLDocument doc; + doc.Parse(stream2.str().c_str()); + + sdf::ParamPassing::modifyChildren( + doc.FirstChildElement("sdf"), + parserConfig, + sdfParsed->Root(), + errors); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "No modifications for element \n provided, " + "skipping modification for:\n\n" + " \n")); + errors.clear(); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::WARN); + sdf::ParamPassing::modifyChildren( + doc.FirstChildElement("sdf"), + parserConfig, + sdfParsed->Root(), + errors); + EXPECT_TRUE(errors.empty()); + // Check the warning has been printed + EXPECT_NE(std::string::npos, buffer.str().find( + "No modifications for element \n provided, " + "skipping modification for:\n\n" + " \n")); +} diff --git a/src/Root.cc b/src/Root.cc index 118d783c0..7e57e5673 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -386,6 +386,9 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) // different frames. checkJointParentChildNames(this, errors); + // Check that //axis*/xyz/@expressed_in values specify valid frames. + checkJointAxisExpressedInValues(this, errors); + return errors; } @@ -435,6 +438,27 @@ bool Root::WorldNameExists(const std::string &_name) const return false; } +///////////////////////////////////////////////// +const World *Root::WorldByName(const std::string &_name) const +{ + for (const sdf::World &w : this->dataPtr->worlds) + { + if (w.Name() == _name) + { + return &w; + } + } + + return nullptr; +} + +///////////////////////////////////////////////// +World *Root::WorldByName(const std::string &_name) +{ + return const_cast( + static_cast(this)->WorldByName(_name)); +} + ///////////////////////////////////////////////// const Model *Root::Model() const { diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index 7988b9ecf..fd169a7c3 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -35,6 +35,7 @@ TEST(DOMRoot, Construction) EXPECT_EQ(nullptr, root.Element()); EXPECT_EQ(SDF_VERSION, root.Version()); EXPECT_FALSE(root.WorldNameExists("default")); + EXPECT_TRUE(root.WorldByName("default") == nullptr); EXPECT_FALSE(root.WorldNameExists("")); EXPECT_EQ(0u, root.WorldCount()); EXPECT_TRUE(root.WorldByIndex(0) == nullptr); @@ -66,6 +67,7 @@ TEST(DOMRoot, MoveAssignmentOperator) EXPECT_EQ("test_root", root2.Version()); } +///////////////////////////////////////////////// TEST(DOMRoot, WorldNamesFromFile) { const auto path = sdf::testing::TestFile("sdf", "basic_shapes.sdf"); @@ -604,3 +606,27 @@ TEST(DOMRoot, CopyConstructor) testFrame1(root3); } } + +///////////////////////////////////////////////// +TEST(DOMRoot, WorldByName) +{ + sdf::Root root; + EXPECT_EQ(0u, root.WorldCount()); + + sdf::World world; + world.SetName("world1"); + EXPECT_TRUE(root.AddWorld(world).empty()); + EXPECT_EQ(1u, root.WorldCount()); + + EXPECT_TRUE(root.WorldNameExists("world1")); + const sdf::World *wPtr = root.WorldByName("world1"); + EXPECT_NE(nullptr, wPtr); + + // Modify the world + sdf::World *w = root.WorldByName("world1"); + ASSERT_NE(nullptr, w); + EXPECT_EQ("world1", w->Name()); + w->SetName("world2"); + ASSERT_TRUE(root.WorldNameExists("world2")); + EXPECT_EQ("world2", root.WorldByName("world2")->Name()); +} diff --git a/src/SDFExtension.cc b/src/SDFExtension.cc index cd72f41fb..53016779a 100644 --- a/src/SDFExtension.cc +++ b/src/SDFExtension.cc @@ -25,6 +25,7 @@ SDFExtension::SDFExtension() this->material.clear(); this->visual_blobs.clear(); this->collision_blobs.clear(); + this->isSetStaticFlag = false; this->setStaticFlag = false; this->isGravity = false; this->gravity = true; @@ -67,52 +68,3 @@ SDFExtension::SDFExtension() this->provideFeedback = false; this->implicitSpringDamper = false; } - -///////////////////////////////////////////////// -SDFExtension::SDFExtension(const SDFExtension &_ge) -{ - this->material = _ge.material; - this->visual_blobs = _ge.visual_blobs; - this->collision_blobs = _ge.collision_blobs; - this->setStaticFlag = _ge.setStaticFlag; - this->isGravity = _ge.isGravity; - this->gravity = _ge.gravity; - this->isDampingFactor = _ge.isDampingFactor; - this->isMaxContacts = _ge.isMaxContacts; - this->isMaxVel = _ge.isMaxVel; - this->isMinDepth = _ge.isMinDepth; - this->fdir1 = _ge.fdir1; - this->isMu1 = _ge.isMu1; - this->isMu2 = _ge.isMu2; - this->isKp = _ge.isKp; - this->isKd = _ge.isKd; - this->selfCollide = _ge.selfCollide; - this->isLaserRetro = _ge.isLaserRetro; - this->isSpringReference = _ge.isSpringReference; - this->isSpringStiffness = _ge.isSpringStiffness; - this->isStopCfm = _ge.isStopCfm; - this->isStopErp = _ge.isStopErp; - this->isFudgeFactor = _ge.isFudgeFactor; - this->isProvideFeedback = _ge.isProvideFeedback; - this->isImplicitSpringDamper = _ge.isImplicitSpringDamper; - this->provideFeedback = _ge.provideFeedback; - this->implicitSpringDamper = _ge.implicitSpringDamper; - this->oldLinkName = _ge.oldLinkName; - this->reductionTransform = _ge.reductionTransform; - this->blobs = _ge.blobs; - - this->dampingFactor = _ge.dampingFactor; - this->maxContacts = _ge.maxContacts; - this->maxVel = _ge.maxVel; - this->minDepth = _ge.minDepth; - this->mu1 = _ge.mu1; - this->mu2 = _ge.mu2; - this->kp = _ge.kp; - this->kd = _ge.kd; - this->laserRetro = _ge.laserRetro; - this->springReference = _ge.springReference; - this->springStiffness = _ge.springStiffness; - this->stopCfm = _ge.stopCfm; - this->stopErp = _ge.stopErp; - this->fudgeFactor = _ge.fudgeFactor; -} diff --git a/src/SDFExtension.hh b/src/SDFExtension.hh index 454480d40..8fe3ac762 100644 --- a/src/SDFExtension.hh +++ b/src/SDFExtension.hh @@ -44,13 +44,6 @@ namespace sdf /// \brief Constructor public: SDFExtension(); - /// \brief Copy constructor - /// \param[in] _ge SDFExtension to copy. - public: SDFExtension(const SDFExtension &_ge); - - /// \brief Destructor - public: virtual ~SDFExtension() = default; - // for reducing fixed joints and removing links public: std::string oldLinkName; public: gz::math::Pose3d reductionTransform; @@ -89,6 +82,7 @@ namespace sdf public: std::vector collision_blobs; // body, default off + public: bool isSetStaticFlag; public: bool setStaticFlag; public: bool isGravity; public: bool gravity; diff --git a/src/Sensor.cc b/src/Sensor.cc index fc146beff..d1b0a2f20 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -691,13 +691,22 @@ Imu *Sensor::ImuSensor() ///////////////////////////////////////////////// sdf::ElementPtr Sensor::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("sensor.sdf", elem); - elem->GetAttribute("type")->Set(this->TypeStr()); - elem->GetAttribute("name")->Set(this->Name()); - sdf::ElementPtr poseElem = elem->GetElement("pose"); + elem->GetAttribute("type")->Set(this->TypeStr(), _errors); + elem->GetAttribute("name")->Set(this->Name(), _errors); + sdf::ElementPtr poseElem = elem->GetElement("pose", _errors); if (!this->dataPtr->poseRelativeTo.empty()) { poseElem->GetAttribute("relative_to")->Set( @@ -760,8 +769,10 @@ sdf::ElementPtr Sensor::ToElement() const } else { - std::cout << "Conversion of sensor type: [" << this->TypeStr() << "] from " - << "SDF DOM to Element is not supported yet." << std::endl; + std::stringstream ss; + ss << "Conversion of sensor type: [" << this->TypeStr() << "] from SDF " + << "DOM to Element is not supported yet." << this->Name(); + _errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); } // Add in the plugins diff --git a/src/World.cc b/src/World.cc index 53504d950..aacb571c9 100644 --- a/src/World.cc +++ b/src/World.cc @@ -25,6 +25,7 @@ #include "sdf/InterfaceElements.hh" #include "sdf/InterfaceModel.hh" #include "sdf/InterfaceModelPoseGraph.hh" +#include "sdf/Joint.hh" #include "sdf/Light.hh" #include "sdf/Model.hh" #include "sdf/ParserConfig.hh" @@ -65,6 +66,9 @@ class sdf::World::Implementation /// \brief The frames specified in this world. public: std::vector frames; + /// \brief The joints specified in this world. + public: std::vector joints; + /// \brief The lights specified in this world. public: std::vector lights; @@ -259,6 +263,11 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) this->dataPtr->actors); errors.insert(errors.end(), actorLoadErrors.begin(), actorLoadErrors.end()); + // Load all the joints. + Errors jointLoadErrors = loadUniqueRepeated(_sdf, "joint", + this->dataPtr->joints); + errors.insert(errors.end(), jointLoadErrors.begin(), jointLoadErrors.end()); + // Load all the lights. Errors lightLoadErrors = loadUniqueRepeated(_sdf, "light", this->dataPtr->lights); @@ -566,6 +575,67 @@ Frame *World::FrameByName(const std::string &_name) static_cast(this)->FrameByName(_name)); } +///////////////////////////////////////////////// +uint64_t World::JointCount() const +{ + return this->dataPtr->joints.size(); +} + +///////////////////////////////////////////////// +const Joint *World::JointByIndex(const uint64_t _index) const +{ + if (_index < this->dataPtr->joints.size()) + return &this->dataPtr->joints[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +Joint *World::JointByIndex(uint64_t _index) +{ + return const_cast( + static_cast(this)->JointByIndex(_index)); +} + +///////////////////////////////////////////////// +const Joint *World::JointByName(const std::string &_name) const +{ + auto index = _name.rfind("::"); + if (index != std::string::npos) + { + const Model *model = this->ModelByName(_name.substr(0, index)); + if (nullptr != model) + { + return model->JointByName(_name.substr(index + 2)); + } + + // The nested model name preceding the last "::" could not be found. + // Since "::" are reserved and not allowed in names, return a nullptr. + return nullptr; + } + + for (auto const &f : this->dataPtr->joints) + { + if (f.Name() == _name) + { + return &f; + } + } + return nullptr; +} + +///////////////////////////////////////////////// +Joint *World::JointByName(const std::string &_name) +{ + return const_cast( + static_cast(this)->JointByName(_name)); +} + +///////////////////////////////////////////////// +bool World::JointNameExists(const std::string &_name) const +{ + return nullptr != this->JointByName(_name); +} + ///////////////////////////////////////////////// uint64_t World::LightCount() const { @@ -728,6 +798,10 @@ void World::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); } + for (auto &joint : this->dataPtr->joints) + { + joint.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } for (auto &light : this->dataPtr->lights) { light.SetXmlParentName("world"); @@ -745,6 +819,10 @@ void World::SetFrameAttachedToGraph( { frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); } + for (auto &joint : this->dataPtr->joints) + { + joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } for (auto &model : this->dataPtr->models) { model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); @@ -920,6 +998,10 @@ sdf::ElementPtr World::ToElement(const OutputConfig &_config) const for (const sdf::Actor &actor : this->dataPtr->actors) elem->InsertElement(actor.ToElement(), true); + // Joints + for (const sdf::Joint &joint : this->dataPtr->joints) + elem->InsertElement(joint.ToElement(), true); + // Lights for (const sdf::Light &light : this->dataPtr->lights) elem->InsertElement(light.ToElement(), true); @@ -985,6 +1067,12 @@ void World::ClearActors() this->dataPtr->actors.clear(); } +///////////////////////////////////////////////// +void World::ClearJoints() +{ + this->dataPtr->joints.clear(); +} + ///////////////////////////////////////////////// void World::ClearLights() { @@ -1003,6 +1091,16 @@ void World::ClearFrames() this->dataPtr->frames.clear(); } +///////////////////////////////////////////////// +bool World::NameExistsInFrameAttachedToGraph(const std::string &_name) const +{ + if (!this->dataPtr->frameAttachedToGraph) + return false; + + return this->dataPtr->frameAttachedToGraph.VertexIdByName(_name) + != gz::math::graph::kNullId; +} + ///////////////////////////////////////////////// bool World::AddModel(const Model &_model) { @@ -1022,6 +1120,16 @@ bool World::AddActor(const Actor &_actor) return true; } +///////////////////////////////////////////////// +bool World::AddJoint(const Joint &_joint) +{ + if (this->JointNameExists(_joint.Name())) + return false; + this->dataPtr->joints.push_back(_joint); + + return true; +} + ///////////////////////////////////////////////// bool World::AddLight(const Light &_light) { diff --git a/src/World_TEST.cc b/src/World_TEST.cc index d8c15faa7..9e9174a4f 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -58,12 +58,28 @@ TEST(DOMWorld, Construction) EXPECT_EQ(nullptr, world.FrameByIndex(1)); EXPECT_FALSE(world.FrameNameExists("")); EXPECT_FALSE(world.FrameNameExists("default")); - - EXPECT_EQ(0u, world.FrameCount()); - EXPECT_EQ(nullptr, world.FrameByIndex(0)); - EXPECT_EQ(nullptr, world.FrameByIndex(1)); - EXPECT_FALSE(world.FrameNameExists("")); - EXPECT_FALSE(world.FrameNameExists("default")); + EXPECT_FALSE(world.FrameNameExists("a::b")); + EXPECT_FALSE(world.FrameNameExists("a::b::c")); + EXPECT_FALSE(world.FrameNameExists("::::")); + EXPECT_EQ(nullptr, world.FrameByName("")); + EXPECT_EQ(nullptr, world.FrameByName("default")); + EXPECT_EQ(nullptr, world.FrameByName("a::b")); + EXPECT_EQ(nullptr, world.FrameByName("a::b::c")); + EXPECT_EQ(nullptr, world.FrameByName("::::")); + + EXPECT_EQ(0u, world.JointCount()); + EXPECT_EQ(nullptr, world.JointByIndex(0)); + EXPECT_EQ(nullptr, world.JointByIndex(1)); + EXPECT_FALSE(world.JointNameExists("")); + EXPECT_FALSE(world.JointNameExists("default")); + EXPECT_FALSE(world.JointNameExists("a::b")); + EXPECT_FALSE(world.JointNameExists("a::b::c")); + EXPECT_FALSE(world.JointNameExists("::::")); + EXPECT_EQ(nullptr, world.JointByName("")); + EXPECT_EQ(nullptr, world.JointByName("default")); + EXPECT_EQ(nullptr, world.JointByName("a::b")); + EXPECT_EQ(nullptr, world.JointByName("a::b::c")); + EXPECT_EQ(nullptr, world.JointByName("::::")); EXPECT_EQ(1u, world.PhysicsCount()); @@ -77,6 +93,10 @@ TEST(DOMWorld, Construction) EXPECT_NE(std::string::npos, errors[1].Message().find( "PoseRelativeToGraph error: scope does not point to a valid graph")); + + // world doesn't have graphs, so no names should exist in graphs + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("model1")); } ///////////////////////////////////////////////// @@ -368,6 +388,9 @@ TEST(DOMWorld, AddModel) EXPECT_EQ(1u, world.ModelCount()); EXPECT_FALSE(world.AddModel(model)); EXPECT_EQ(1u, world.ModelCount()); + // world doesn't have graphs, so no names should exist in graphs + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("model1")); world.ClearModels(); EXPECT_EQ(0u, world.ModelCount()); @@ -435,6 +458,29 @@ TEST(DOMWorld, AddActor) EXPECT_EQ(actorFromWorld->Name(), actor.Name()); } +///////////////////////////////////////////////// +TEST(DOMWorld, AddJoint) +{ + sdf::World world; + EXPECT_EQ(0u, world.JointCount()); + + sdf::Joint joint; + joint.SetName("joint1"); + EXPECT_TRUE(world.AddJoint(joint)); + EXPECT_EQ(1u, world.JointCount()); + EXPECT_FALSE(world.AddJoint(joint)); + EXPECT_EQ(1u, world.JointCount()); + + world.ClearJoints(); + EXPECT_EQ(0u, world.JointCount()); + + EXPECT_TRUE(world.AddJoint(joint)); + EXPECT_EQ(1u, world.JointCount()); + const sdf::Joint *jointFromWorld = world.JointByIndex(0); + ASSERT_NE(nullptr, jointFromWorld); + EXPECT_EQ(jointFromWorld->Name(), joint.Name()); +} + ///////////////////////////////////////////////// TEST(DOMWorld, AddLight) { @@ -515,6 +561,19 @@ TEST(DOMWorld, ToElement) world.ClearActors(); } + for (int j = 0; j <= 1; ++j) + { + for (int i = 0; i < 4; ++i) + { + sdf::Joint joint; + joint.SetName("joint" + std::to_string(i)); + EXPECT_TRUE(world.AddJoint(joint)); + EXPECT_FALSE(world.AddJoint(joint)); + } + if (j == 0) + world.ClearJoints(); + } + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 4; ++i) @@ -572,6 +631,10 @@ TEST(DOMWorld, ToElement) for (uint64_t i = 0; i < world2.ModelCount(); ++i) EXPECT_NE(nullptr, world2.ModelByIndex(i)); + EXPECT_EQ(world.JointCount(), world2.JointCount()); + for (uint64_t i = 0; i < world2.JointCount(); ++i) + EXPECT_NE(nullptr, world2.JointByIndex(i)); + EXPECT_EQ(world.LightCount(), world2.LightCount()); for (uint64_t i = 0; i < world2.LightCount(); ++i) EXPECT_NE(nullptr, world2.LightByIndex(i)); @@ -628,6 +691,10 @@ TEST(DOMWorld, MutableByIndex) actor.SetName("actor1"); EXPECT_TRUE(world.AddActor(actor)); + sdf::Joint joint; + joint.SetName("joint1"); + EXPECT_TRUE(world.AddJoint(joint)); + sdf::Light light; light.SetName("light1"); EXPECT_TRUE(world.AddLight(light)); @@ -654,6 +721,13 @@ TEST(DOMWorld, MutableByIndex) a->SetName("actor2"); EXPECT_EQ("actor2", world.ActorByIndex(0)->Name()); + // Modify the joint + sdf::Joint *j = world.JointByIndex(0); + ASSERT_NE(nullptr, j); + EXPECT_EQ("joint1", j->Name()); + j->SetName("joint2"); + EXPECT_EQ("joint2", world.JointByIndex(0)->Name()); + // Modify the light sdf::Light *l = world.LightByIndex(0); ASSERT_NE(nullptr, l); @@ -689,6 +763,10 @@ TEST(DOMWorld, MutableByName) frame.SetName("frame1"); EXPECT_TRUE(world.AddFrame(frame)); + sdf::Joint joint; + joint.SetName("joint1"); + EXPECT_TRUE(world.AddJoint(joint)); + // Modify the model sdf::Model *m = world.ModelByName("model1"); ASSERT_NE(nullptr, m); @@ -704,6 +782,14 @@ TEST(DOMWorld, MutableByName) f->SetName("frame2"); EXPECT_FALSE(world.FrameByName("frame1")); EXPECT_TRUE(world.FrameByName("frame2")); + + // Modify the joint + sdf::Joint *j = world.JointByName("joint1"); + ASSERT_NE(nullptr, j); + EXPECT_EQ("joint1", j->Name()); + j->SetName("joint2"); + EXPECT_FALSE(world.JointByName("joint1")); + EXPECT_TRUE(world.JointByName("joint2")); } ///////////////////////////////////////////////// diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 8d1e4f585..9d4bde234 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -355,6 +355,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) EXPECT_NE(output.find("Joint with name[joint] specified invalid " "child link [world]."), std::string::npos) << output; + EXPECT_NE(output.find("Child frame with name[world] specified by joint " + "with name[joint] not found in model"), + std::string::npos) << output; } // Check an SDF file with the world specified as a parent link. diff --git a/src/parser.cc b/src/parser.cc index f5cd07786..ec36af87a 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -28,6 +28,7 @@ #include "sdf/Filesystem.hh" #include "sdf/Frame.hh" #include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" #include "sdf/Link.hh" #include "sdf/Model.hh" #include "sdf/Param.hh" @@ -952,7 +953,7 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf, && strcmp(sdfNode->Attribute("version"), SDF::Version().c_str()) != 0) { sdfdbg << "Converting a deprecated source[" << _source << "].\n"; - Converter::Convert(_xmlDoc, SDF::Version()); + Converter::Convert(_errors, _xmlDoc, SDF::Version(), _config); } auto *elemXml = _xmlDoc->FirstChildElement(_sdf->Root()->GetName().c_str()); @@ -1042,7 +1043,7 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf, { sdfdbg << "Converting a deprecated SDF source[" << _source << "].\n"; - Converter::Convert(_xmlDoc, SDF::Version()); + Converter::Convert(_errors, _xmlDoc, SDF::Version(), _config); } tinyxml2::XMLElement *elemXml = sdfNode; @@ -1944,18 +1945,34 @@ bool convertFile(const std::string &_filename, const std::string &_version, bool convertFile(const std::string &_filename, const std::string &_version, const ParserConfig &_config, SDFPtr _sdf) { + sdf::Errors errors = convertFile(_sdf, _filename, _version, _config); + throwOrPrintErrors(errors); + return errors.empty(); +} + +///////////////////////////////////////////////// +sdf::Errors convertFile(SDFPtr _sdf, const std::string &_filename, + const std::string &_version, + const ParserConfig &_config) +{ + sdf::Errors errors; std::string filename = sdf::findFile(_filename, true, false, _config); if (filename.empty()) { - sdferr << "Error finding file [" << _filename << "].\n"; - return false; + std::stringstream ss; + ss << "Error finding file [" + << _filename + << "]."; + errors.push_back({ErrorCode::FILE_READ, ss.str()}); + return errors; } if (nullptr == _sdf || nullptr == _sdf->Root()) { - sdferr << "SDF pointer or its Root is null.\n"; - return false; + errors.push_back({ErrorCode::CONVERSION_ERROR, + "SDF pointer or its Root is null."}); + return errors; } auto xmlDoc = makeSdfDoc(); @@ -1973,25 +1990,26 @@ bool convertFile(const std::string &_filename, const std::string &_version, _sdf->SetOriginalVersion(originalVersion); - if (sdf::Converter::Convert(&xmlDoc, _version, true)) + if (sdf::Converter::Convert(errors, &xmlDoc, _version, _config, true)) { - Errors errors; bool result = sdf::readDoc(&xmlDoc, _sdf, filename, false, _config, errors); - - // Output errors - for (auto const &e : errors) - std::cerr << e << std::endl; - - return result; + if (!result) + { + std::stringstream ss; + ss << "Error in sdf::readDoc when parsing file[" << filename << "]"; + errors.push_back({ErrorCode::PARSING_ERROR, ss.str()}); + } } } else { - sdferr << "Error parsing file[" << filename << "]\n"; + std::stringstream ss; + ss << "Error parsing file[" << filename << "]"; + errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); } - return false; + return errors; } ///////////////////////////////////////////////// @@ -2006,10 +2024,22 @@ bool convertString(const std::string &_sdfString, const std::string &_version, bool convertString(const std::string &_sdfString, const std::string &_version, const ParserConfig &_config, SDFPtr _sdf) { + sdf::Errors errors = convertString(_sdf, _sdfString, _version, _config); + throwOrPrintErrors(errors); + return errors.empty(); +} + +///////////////////////////////////////////////// +sdf::Errors convertString(SDFPtr _sdf, const std::string &_sdfString, + const std::string &_version, + const ParserConfig &_config) +{ + sdf::Errors errors; + if (_sdfString.empty()) { - sdferr << "SDF string is empty.\n"; - return false; + errors.push_back({ErrorCode::CONVERSION_ERROR, "SDF string is empty."}); + return errors; } tinyxml2::XMLDocument xmlDoc; @@ -2029,25 +2059,30 @@ bool convertString(const std::string &_sdfString, const std::string &_version, _sdf->SetOriginalVersion(originalVersion); - if (sdf::Converter::Convert(&xmlDoc, _version, true)) + if (sdf::Converter::Convert(errors, &xmlDoc, _version, _config, true)) { - Errors errors; bool result = sdf::readDoc(&xmlDoc, _sdf, std::string(kSdfStringSource), false, _config, errors); - - // Output errors - for (auto const &e : errors) - std::cerr << e << std::endl; - - return result; + if (!result) + { + std::stringstream ss; + ss << "Error in sdf::readDoc when parsing XML from string[" + << _sdfString + << "]"; + errors.push_back({ErrorCode::PARSING_ERROR, ss.str()}); + } } } else { - sdferr << "Error parsing XML from string[" << _sdfString << "]\n"; + std::stringstream ss; + ss << "Error parsing XML from string[" + << _sdfString + << "]"; + errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); } - return false; + return errors; } ////////////////////////////////////////////////// @@ -2502,87 +2537,157 @@ bool checkJointParentChildNames(const sdf::Root *_root) } ////////////////////////////////////////////////// -void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) +template +void checkScopedJointParentChildNames( + const TPtr _scope, const std::string &_scopeType, Errors &errors) { - auto checkModelJointParentChildNames = []( - const sdf::Model *_model, Errors &errors) -> void + for (uint64_t j = 0; j < _scope->JointCount(); ++j) { - for (uint64_t j = 0; j < _model->JointCount(); ++j) + auto joint = _scope->JointByIndex(j); + + const std::string &parentName = joint->ParentName(); + const std::string parentLocalName = sdf::SplitName(parentName).second; + + if (parentName != "world" && parentLocalName != "__model__" && + !_scope->NameExistsInFrameAttachedToGraph(parentName)) { - auto joint = _model->JointByIndex(j); + errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, + "parent frame with name[" + parentName + + "] specified by joint with name[" + joint->Name() + + "] not found in " + _scopeType + " with name[" + + _scope->Name() + "]."}); + } - const std::string &parentName = joint->ParentName(); - const std::string parentLocalName = sdf::SplitName(parentName).second; + const std::string &childName = joint->ChildName(); + const std::string childLocalName = sdf::SplitName(childName).second; + if (childName == "world") + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "invalid child name[world] specified by joint with name[" + + joint->Name() + "] in " + _scopeType + " with name[" + + _scope->Name() + "]."}); + } - if (parentName != "world" && parentLocalName != "__model__" && - !_model->NameExistsInFrameAttachedToGraph(parentName)) - { - errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, - "parent frame with name[" + parentName + - "] specified by joint with name[" + joint->Name() + - "] not found in model with name[" + _model->Name() + "]."}); - } + if (childLocalName != "__model__" && + !_scope->NameExistsInFrameAttachedToGraph(childName)) + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "child frame with name[" + childName + + "] specified by joint with name[" + joint->Name() + + "] not found in " + _scopeType + " with name[" + + _scope->Name() + "]."}); + } - const std::string &childName = joint->ChildName(); - const std::string childLocalName = sdf::SplitName(childName).second; - if (childName == "world") - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "invalid child name[world] specified by joint with name[" + - joint->Name() + "] in model with name[" + _model->Name() + "]."}); - } + if (childName == joint->Name()) + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " with name[" + _scope->Name() + + "] must not specify its own name as the child frame."}); + } - if (childLocalName != "__model__" && - !_model->NameExistsInFrameAttachedToGraph(childName)) - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "child frame with name[" + childName + - "] specified by joint with name[" + joint->Name() + - "] not found in model with name[" + _model->Name() + "]."}); - } + if (parentName == joint->Name()) + { + errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " with name[" + _scope->Name() + + "] must not specify its own name as the parent frame."}); + } - if (childName == joint->Name()) - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "joint with name[" + joint->Name() + - "] in model with name[" + _model->Name() + - "] must not specify its own name as the child frame."}); - } + // Check that parent and child frames resolve to different links + std::string resolvedChildName; + std::string resolvedParentName; - if (parentName == joint->Name()) - { - errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, - "joint with name[" + joint->Name() + - "] in model with name[" + _model->Name() + - "] must not specify its own name as the parent frame."}); - } + auto resolveErrors = joint->ResolveChildLink(resolvedChildName); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + + resolveErrors = joint->ResolveParentLink(resolvedParentName); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - // Check that parent and child frames resolve to different links - std::string resolvedChildName; - std::string resolvedParentName; + if (resolvedChildName == resolvedParentName) + { + errors.push_back({ErrorCode::JOINT_PARENT_SAME_AS_CHILD, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " with name[" + _scope->Name() + + "] specified parent frame [" + parentName + + "] and child frame [" + childName + + "] that both resolve to [" + resolvedChildName + + "], but they should resolve to different values."}); + } + // childName == "world" case is handled above + if (childName != "world" && resolvedChildName == "world") + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " has a child with name[" + joint->ChildName() + + "] that resolves to world which is invalid."}); + } + } +} - auto resolveErrors = joint->ResolveChildLink(resolvedChildName); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); +////////////////////////////////////////////////// +void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) +{ + if (_root->Model()) + { + checkScopedJointParentChildNames(_root->Model(), "model", _errors); + } - resolveErrors = joint->ResolveParentLink(resolvedParentName); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + for (uint64_t w = 0; w < _root->WorldCount(); ++w) + { + auto world = _root->WorldByIndex(w); + for (uint64_t m = 0; m < world->ModelCount(); ++m) + { + auto model = world->ModelByIndex(m); + checkScopedJointParentChildNames(model, "model", _errors); + } + checkScopedJointParentChildNames(world, "world", _errors); + } +} - if (resolvedChildName == resolvedParentName) +////////////////////////////////////////////////// +template +void checkScopedJointAxisExpressedInValues( + const TPtr _scope, const std::string &_scopeType, Errors &errors) +{ + for (uint64_t j = 0; j < _scope->JointCount(); ++j) + { + auto joint = _scope->JointByIndex(j); + for (uint64_t a = 0; a < 2; ++a) + { + auto axis = joint->Axis(a); + if (axis) { - errors.push_back({ErrorCode::JOINT_PARENT_SAME_AS_CHILD, - "joint with name[" + joint->Name() + - "] in model with name[" + _model->Name() + - "] specified parent frame [" + parentName + - "] and child frame [" + childName + - "] that both resolve to [" + resolvedChildName + - "], but they should resolve to different values."}); + const std::string &xyzExpressedIn = axis->XyzExpressedIn(); + const std::string xyzExpressedInLocal = + sdf::SplitName(xyzExpressedIn).second; + + // If a frame name is specfied, check that the frame exists. + if (!xyzExpressedIn.empty() && + !_scope->NameExistsInFrameAttachedToGraph(xyzExpressedIn)) + { + errors.push_back({ErrorCode::JOINT_AXIS_EXPRESSED_IN_INVALID, + "axis xyz expressed-in frame with name[" + xyzExpressedIn + + "] specified by joint with name[" + joint->Name() + + "] not found in " + _scopeType + " with name[" + _scope->Name() + + "]."}); + } + + // Also try resolving the axis to its default frame. + gz::math::Vector3d xyz; + auto xyzErrors = axis->ResolveXyz(xyz); + errors.insert(errors.end(), xyzErrors.begin(), xyzErrors.end()); } } - }; + } +} +////////////////////////////////////////////////// +void checkJointAxisExpressedInValues(const sdf::Root *_root, Errors &_errors) +{ if (_root->Model()) { - checkModelJointParentChildNames(_root->Model(), _errors); + checkScopedJointAxisExpressedInValues(_root->Model(), "model", _errors); } for (uint64_t w = 0; w < _root->WorldCount(); ++w) @@ -2591,8 +2696,9 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) for (uint64_t m = 0; m < world->ModelCount(); ++m) { auto model = world->ModelByIndex(m); - checkModelJointParentChildNames(model, _errors); + checkScopedJointAxisExpressedInValues(model, "model", _errors); } + checkScopedJointAxisExpressedInValues(world, "world", _errors); } } diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index c2660ae9b..f0f2b8b90 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -1363,6 +1363,7 @@ void URDF2SDF::ParseSDFExtension(tinyxml2::XMLDocument &_urdfXml) else if (strcmp(childElem->Name(), "static") == 0) { std::string valueStr = GetKeyValueAsString(childElem); + sdf->isSetStaticFlag = true; // default of setting static flag is false if (lowerStr(valueStr) == "true" || lowerStr(valueStr) == "yes" || @@ -2311,14 +2312,17 @@ void InsertSDFExtensionRobot(tinyxml2::XMLElement *_elem) for (std::vector::iterator ge = sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge) { - // insert static flag - if ((*ge)->setStaticFlag) + if ((*ge)->isSetStaticFlag) { - AddKeyValue(_elem, "static", "true"); - } - else - { - AddKeyValue(_elem, "static", "false"); + // insert static flag + if ((*ge)->setStaticFlag) + { + AddKeyValue(_elem, "static", "true"); + } + else + { + AddKeyValue(_elem, "static", "false"); + } } // copy extension containing blobs and without reference diff --git a/test/integration/error_output.cc b/test/integration/error_output.cc index c00c599a0..c4607fc1d 100644 --- a/test/integration/error_output.cc +++ b/test/integration/error_output.cc @@ -22,7 +22,10 @@ #include "sdf/Element.hh" #include "sdf/Error.hh" +#include "sdf/Model.hh" #include "sdf/Param.hh" +#include "sdf/Sensor.hh" +#include "sdf/parser.hh" #include "sdf/Types.hh" #include "sdf/World.hh" #include "test_utils.hh" @@ -36,6 +39,15 @@ TEST(ErrorOutput, ParamErrorOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; ASSERT_NO_THROW(sdf::Param param1("key", "not_valid_type", "true", false, errors, "description")); @@ -158,6 +170,15 @@ TEST(ErrorOutput, ElementErrorOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; sdf::ElementPtr elem = std::make_shared(); elem->SetName("testElement"); @@ -230,6 +251,15 @@ TEST(ErrorOutput, PrintConfigErrorOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; sdf::PrintConfig printConfig; ASSERT_FALSE(printConfig.SetRotationSnapToDegrees(361, 300, errors)); @@ -256,6 +286,15 @@ TEST(ErrorOutput, WorldErrorOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; std::ostringstream stream; @@ -296,3 +335,223 @@ TEST(ErrorOutput, WorldErrorOutput) // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } + +//////////////////////////////////////// +// Test Sensor class for sdf::Errors outputs +TEST(ErrorOutput, SensorErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + sdf::Errors errors; + sdf::Sensor sensor; + sensor.ToElement(errors); + ASSERT_EQ(errors.size(), 2u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Empty string used when setting a required parameter. Key[name]")); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "Conversion of sensor type: [none] from SDF DOM to Element is not" + " supported yet.")); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} + +//////////////////////////////////////// +// Test Model class for sdf::Errors outputs +TEST(ErrorOutput, ModelErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::Errors errors; + + std::function findFileCb = [](const std::string &_uri) + { + return sdf::testing::TestFile("integration", "model", _uri); + }; + + std::ostringstream stream; + stream << "" + << "" + << " " + << " " + << " test_model" + << " common_name" + << " " + << " " + << " " + << " " + << " world" + << " child" + << " " + << " " + << " world" + << " child" + << " " + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + parserConfig.SetFindCallback(findFileCb); + sdf::readString(stream.str(), parserConfig, sdfParsed, errors); + EXPECT_TRUE(errors.empty()); + + { + sdf::Model model; + errors = model.Load(sdfParsed->Root()->GetElement("model"), parserConfig); + ASSERT_EQ(errors.size(), 7u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Non-unique name[common_name] detected 7 times in XML children of model" + " with name[test_model].")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "link with name[common_name] already exists.")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "Link with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this link.")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[3].Message().find( + "joint with name[common_name] already exists.")); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[4].Message().find( + "Joint with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this joint.")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[5].Message().find( + "frame with name[common_name] already exists.")); + EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[6].Message().find( + "Frame with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this frame.")); + errors.clear(); + } + + { + sdf::Model model; + // Check warnings are still printed when the policy is not set to error. + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::WARN); + errors = model.Load(sdfParsed->Root()->GetElement("model"), parserConfig); + ASSERT_EQ(errors.size(), 6u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "link with name[common_name] already exists.")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "Link with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this link.")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "joint with name[common_name] already exists.")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[3].Message().find( + "Joint with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this joint.")); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[4].Message().find( + "frame with name[common_name] already exists.")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[5].Message().find( + "Frame with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this frame.")); + // Check printed warnings + EXPECT_NE(std::string::npos, buffer.str().find( + "Non-unique name[common_name] detected 7 times in XML children of model" + " with name[test_model].")) + << buffer.str(); + buffer.str(""); + buffer.clear(); + errors.clear(); + } + + { + sdf::Model model; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + // Set SDF version to someting lower than 1.7 for extra errors + sdfParsed->Root()->GetElement("model")->SetOriginalVersion("1.6"); + errors = model.Load(sdfParsed->Root()->GetElement("model"), parserConfig); + + ASSERT_EQ(errors.size(), 7u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Non-unique name[common_name] detected 7 times in XML children of model" + " with name[test_model].")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "link with name[common_name] already exists.")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "Link with name [common_name] in model with name [test_model] has a " + "name collision, changing link name to [common_name_link].")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[3].Message().find( + "joint with name[common_name] already exists.")); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[4].Message().find( + "Joint with name [common_name] in model with name [test_model] has a " + "name collision, changing joint name to [common_name_joint].")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[5].Message().find( + "frame with name[common_name] already exists.")); + EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[6].Message().find( + "Frame with name [common_name] in model with name [test_model] has a " + "name collision, changing frame name to [common_name_frame].")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + errors.clear(); + } + + { + sdf::Model model; + // Check warnings are still printed when the policy is not set to error. + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::WARN); + errors = model.Load(sdfParsed->Root()->GetElement("model"), parserConfig); + ASSERT_EQ(errors.size(), 3u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "link with name[common_name] already exists.")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "joint with name[common_name] already exists.")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "frame with name[common_name] already exists.")); + // Check printed warnings + EXPECT_NE(std::string::npos, buffer.str().find( + "Non-unique name[common_name] detected 7 times in XML children of model" + " with name[test_model].")) + << buffer.str(); + EXPECT_NE(std::string::npos, buffer.str().find( + "Link with name [common_name] in model with name [test_model] has a " + "name collision, changing link name to [common_name_link].")) + << buffer.str(); + EXPECT_NE(std::string::npos, buffer.str().find( + "Joint with name [common_name] in model with name [test_model] has a " + "name collision, changing joint name to [common_name_joint].")) + << buffer.str(); + EXPECT_NE(std::string::npos, buffer.str().find( + "Frame with name [common_name] in model with name [test_model] has a " + "name collision, changing frame name to [common_name_frame].")) + << buffer.str(); + } +} diff --git a/test/integration/fixed_joint_static.urdf b/test/integration/fixed_joint_static.urdf new file mode 100644 index 000000000..8058ea6c8 --- /dev/null +++ b/test/integration/fixed_joint_static.urdf @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc index b1c102e28..d8986adfa 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -219,6 +219,26 @@ TEST(DOMJointAxis, XyzExpressedIn) EXPECT_EQ(nullptr, model->FrameByIndex(0)); } +///////////////////////////////////////////////// +TEST(DOMJointAxis, InvalidExpressedIn) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "joint_axis_invalid_expressed_in.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (auto e : errors) + std::cout << e << std::endl; + ASSERT_EQ(2u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_AXIS_EXPRESSED_IN_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "axis xyz expressed-in frame with name[invalid] specified by joint with" + " name[joint] not found in model with" + " name[joint_axis_invalid_expressed_in]")); +} + ////////////////////////////////////////////////// TEST(DOMJointAxis, InfiniteLimits) { diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 2dffa07be..7ca33c506 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -30,6 +30,7 @@ #include "sdf/SDFImpl.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" +#include "sdf/World.hh" #include "sdf/parser.hh" #include "test_config.hh" @@ -157,6 +158,49 @@ TEST(DOMJoint, Complete) } } +////////////////////////////////////////////////// +TEST(DOMJoint, ScrewThreadPitch) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "joint_screw_thread_pitch.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + + EXPECT_EQ(2u, model->LinkCount()); + EXPECT_TRUE(model->LinkNameExists("child_link")); + EXPECT_TRUE(model->LinkNameExists("parent_link")); + + EXPECT_EQ(5u, model->JointCount()); + ASSERT_TRUE(model->JointNameExists("default_param")); + ASSERT_TRUE(model->JointNameExists("both_params")); + ASSERT_TRUE(model->JointNameExists("new_param")); + ASSERT_TRUE(model->JointNameExists("old_param")); + ASSERT_TRUE(model->JointNameExists("param_precedence")); + + EXPECT_DOUBLE_EQ(1.0, + model->JointByName("default_param")->ScrewThreadPitch()); + EXPECT_DOUBLE_EQ(0.5, model->JointByName("both_params")->ScrewThreadPitch()); + EXPECT_DOUBLE_EQ(0.5, model->JointByName("new_param")->ScrewThreadPitch()); + EXPECT_NEAR(0.5, model->JointByName("old_param")->ScrewThreadPitch(), 1e-3); + EXPECT_DOUBLE_EQ(0.5, + model->JointByName("param_precedence")->ScrewThreadPitch()); + + EXPECT_DOUBLE_EQ(-2*GZ_PI, + model->JointByName("default_param")->ThreadPitch()); + EXPECT_NEAR(-12.566, model->JointByName("both_params")->ThreadPitch(), 1e-3); + EXPECT_NEAR(-12.566, model->JointByName("new_param")->ThreadPitch(), 1e-3); + EXPECT_DOUBLE_EQ(-12.566, model->JointByName("old_param")->ThreadPitch()); + EXPECT_NEAR(-12.566, model->JointByName("param_precedence")->ThreadPitch(), + 1e-3); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadJointParentWorld) { @@ -356,7 +400,7 @@ TEST(DOMJoint, LoadJointParentFrame) EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), model->FrameByName("parent_frame")->RawPose()); - // Test ResolveFrame to get each link, joint and frame pose in model frame. + // Test Resolve to get each link, joint and frame pose in model frame. Pose pose; EXPECT_TRUE( model->LinkByName("parent_link")-> @@ -448,7 +492,7 @@ TEST(DOMJoint, LoadJointChildFrame) EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), model->FrameByName("child_frame")->RawPose()); - // Test ResolveFrame to get each link, joint and frame pose in model frame. + // Test Resolve to get each link, joint and frame pose in model frame. Pose pose; EXPECT_TRUE( model->LinkByName("parent_link")-> @@ -478,6 +522,158 @@ TEST(DOMJoint, LoadJointChildFrame) EXPECT_EQ(Pose(1, 1, 9, 0, 0, 0), pose); } +///////////////////////////////////////////////// +TEST(DOMJoint, LoadWorldJointChildFrame) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_joint_child_frame.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + using Pose = gz::math::Pose3d; + + // Get the world + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ("world_joint_child_frame", world->Name()); + EXPECT_EQ(2u, world->ModelCount()); + EXPECT_NE(nullptr, world->ModelByIndex(0)); + EXPECT_NE(nullptr, world->ModelByIndex(1)); + EXPECT_EQ(nullptr, world->ModelByIndex(2)); + + ASSERT_TRUE(world->ModelNameExists("parent_model")); + ASSERT_TRUE(world->ModelNameExists("child_model")); + EXPECT_TRUE(world->ModelByName("parent_model")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->ModelByName("child_model")->PoseRelativeTo().empty()); + + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), + world->ModelByName("parent_model")->RawPose()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), + world->ModelByName("child_model")->RawPose()); + + EXPECT_EQ(2u, world->JointCount()); + EXPECT_NE(nullptr, world->JointByIndex(0)); + EXPECT_NE(nullptr, world->JointByIndex(1)); + EXPECT_EQ(nullptr, world->JointByIndex(2)); + ASSERT_TRUE(world->JointNameExists("J1")); + ASSERT_TRUE(world->JointNameExists("J2")); + EXPECT_EQ("child_frame", world->JointByName("J1")->ChildName()); + EXPECT_EQ("parent_model::L", world->JointByName("J1")->ParentName()); + EXPECT_EQ("child_model::L", world->JointByName("J2")->ChildName()); + EXPECT_EQ("parent_model::L", world->JointByName("J2")->ParentName()); + + std::string resolvedLinkName; + EXPECT_TRUE( + world->JointByName("J1")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("child_model::L", resolvedLinkName); + EXPECT_TRUE( + world->JointByName("J1")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("parent_model::L", resolvedLinkName); + EXPECT_TRUE( + world->JointByName("J2")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("child_model::L", resolvedLinkName); + EXPECT_TRUE( + world->JointByName("J2")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("parent_model::L", resolvedLinkName); + + EXPECT_TRUE(world->JointByName("J1")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->JointByName("J2")->PoseRelativeTo().empty()); + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), world->JointByName("J1")->RawPose()); + EXPECT_EQ(Pose(10, 0, 0, 0, 0, 0), world->JointByName("J2")->RawPose()); + + EXPECT_EQ(1u, world->FrameCount()); + EXPECT_NE(nullptr, world->FrameByIndex(0)); + EXPECT_EQ(nullptr, world->FrameByIndex(1)); + + ASSERT_TRUE(world->FrameNameExists("child_frame")); + + EXPECT_EQ(Pose(0, 1, 0, 0, 0, 0), + world->FrameByName("child_frame")->RawPose()); + + // Test Resolve to get each model, joint and frame pose in world frame. + Pose pose; + EXPECT_TRUE( + world->ModelByName("parent_model")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), pose); + EXPECT_TRUE( + world->ModelByName("child_model")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J1")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(1, 1, 10, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J2")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(10, 0, 10, 0, 0, 0), pose); + EXPECT_TRUE( + world->FrameByName("child_frame")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(0, 1, 10, 0, 0, 0), pose); + + // joint frame relative to parent and child models + EXPECT_TRUE( + world->JointByName("J1")-> + SemanticPose().Resolve(pose, "child_frame").empty()); + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J1")-> + SemanticPose().Resolve(pose, "parent_model::L").empty()); + EXPECT_EQ(Pose(1, 1, 9, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J2")-> + SemanticPose().Resolve(pose, "child_model::L").empty()); + EXPECT_EQ(Pose(10, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J2")-> + SemanticPose().Resolve(pose, "parent_model::L").empty()); + EXPECT_EQ(Pose(10, 0, 9, 0, 0, 0), pose); +} + +///////////////////////////////////////////////// +TEST(DOMJoint, WorldJointInvalidChildWorld) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_joint_invalid_child_world.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (auto e : errors) + std::cout << e << std::endl; + ASSERT_EQ(4u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "Joint with name[J2] specified invalid child link [world]")); +} + +///////////////////////////////////////////////// +TEST(DOMJoint, WorldJointInvalidResolvedParentSameAsChild) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", + "world_joint_invalid_resolved_parent_same_as_child.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + std::cerr << errors << std::endl; + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_PARENT_SAME_AS_CHILD); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "joint with name[J] in world with name[" + "joint_invalid_resolved_parent_same_as_child.sdf] specified parent " + "frame [child_model] and child frame [child_frame] that both resolve " + "to [child_model::L]")); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadJointPoseRelativeTo) { @@ -531,7 +727,7 @@ TEST(DOMJoint, LoadJointPoseRelativeTo) EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), model->JointByName("J1")->RawPose()); EXPECT_EQ(Pose(0, 0, 2, 0, 0, 0), model->JointByName("J2")->RawPose()); - // Test ResolveFrame to get each link and joint pose in the model frame. + // Test Resolve to get each link and joint pose in the model frame. Pose pose; EXPECT_TRUE( model->LinkByName("P1")-> @@ -650,15 +846,15 @@ TEST(DOMJoint, LoadLinkJointSameName17Invalid) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(9u, errors.size()); - EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_EQ(10u, errors.size()); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, - errors[0].Message().find( + errors[1].Message().find( "Joint with non-unique name [attachment] detected in model with name " "[link_joint_same_name].")); - EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, - errors[3].Message().find( + errors[4].Message().find( "Joint with non-unique name [attachment] detected in model with name " "[link_joint_same_name].")); } @@ -710,7 +906,7 @@ TEST(DOMJoint, LoadLinkJointSameName16Valid) EXPECT_EQ(Pose(0, 0, 3, 0, 0, 0), model->JointByName("attachment_joint")->RawPose()); - // Test ResolveFrame to get each link and joint pose in the model frame. + // Test Resolve to get each link and joint pose in the model frame. Pose pose; EXPECT_TRUE( model->LinkByName("base")-> diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index e549afc0c..57751b3f3 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -318,6 +318,7 @@ TEST(DOMLink, Sensors) EXPECT_EQ("", cameraSensor->Topic()); EXPECT_EQ("my_camera/trigger", camSensor->TriggerTopic()); EXPECT_TRUE(camSensor->Triggered()); + EXPECT_EQ("/camera_sensor/camera_info", camSensor->CameraInfoTopic()); EXPECT_EQ(640u, camSensor->ImageWidth()); EXPECT_EQ(480u, camSensor->ImageHeight()); EXPECT_EQ(sdf::PixelFormatType::RGB_INT8, camSensor->PixelFormat()); diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 4182c136e..ec9181c4e 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -375,6 +375,17 @@ TEST(NestedModel, NestedInclude) ASSERT_NE(nullptr, upperJoint1); ASSERT_NE(nullptr, upperJoint2); ASSERT_NE(nullptr, upperJoint3); + // test World::JointByName + EXPECT_EQ(lowerJoint1, world->JointByName(name + "::lower_joint")); + EXPECT_EQ(lowerJoint2, world->JointByName( + "include_with_rotation::" + name + "::lower_joint")); + EXPECT_EQ(lowerJoint3, world->JointByName( + "include_with_rotation_1.4::" + name + "_14::lower_joint")); + EXPECT_EQ(upperJoint1, world->JointByName(name + "::upper_joint")); + EXPECT_EQ(upperJoint2, world->JointByName( + "include_with_rotation::" + name + "::upper_joint")); + EXPECT_EQ(upperJoint3, world->JointByName( + "include_with_rotation_1.4::" + name + "_14::upper_joint")); const sdf::JointAxis *lowerAxis1 = lowerJoint1->Axis(0); const sdf::JointAxis *lowerAxis2 = lowerJoint2->Axis(0); diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 541ac5217..a8c9f7f7c 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -35,6 +35,10 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) auto modelDom = root.Model(); ASSERT_NE(nullptr, modelDom); + + // Verify that model is not static + EXPECT_FALSE(modelDom->Static()); + sdf::ElementPtr model = modelDom->Element(); for (sdf::ElementPtr joint = model->GetElement("joint"); joint; joint = joint->GetNextElement("joint")) @@ -379,6 +383,39 @@ TEST(SDFParser, FixedJointSimple) auto model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("fixed_joint_simple", model->Name()); + EXPECT_FALSE(model->Static()); + + EXPECT_EQ(1u, model->LinkCount()); + EXPECT_TRUE(model->LinkNameExists("base")); + + auto link = model->LinkByName("base"); + ASSERT_NE(nullptr, link); + auto massMatrix = link->Inertial().MassMatrix(); + EXPECT_DOUBLE_EQ(2.0, massMatrix.Mass()); + EXPECT_EQ(2.0 * gz::math::Matrix3d::Identity, massMatrix.Moi()); + + EXPECT_EQ(0u, model->JointCount()); + + EXPECT_EQ(2u, model->FrameCount()); + ASSERT_TRUE(model->FrameNameExists("fixed_joint")); + ASSERT_TRUE(model->FrameNameExists("child_link")); +} + +///////////////////////////////////////////////// +TEST(SDFParser, FixedJointStatic) +{ + const std::string urdfTestFile = + sdf::testing::TestFile("integration", "fixed_joint_static.urdf"); + + sdf::Root root; + auto errors = root.Load(urdfTestFile); + EXPECT_TRUE(errors.empty()) << errors; + + auto model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("fixed_joint_simple", model->Name()); + + EXPECT_TRUE(model->Static()); EXPECT_EQ(1u, model->LinkCount()); EXPECT_TRUE(model->LinkNameExists("base")); diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index bfbde44e2..9f29e3464 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -1,6 +1,11 @@ + + + 0 + + diff --git a/test/sdf/joint_axis_invalid_expressed_in.sdf b/test/sdf/joint_axis_invalid_expressed_in.sdf new file mode 100644 index 000000000..c9524f6cd --- /dev/null +++ b/test/sdf/joint_axis_invalid_expressed_in.sdf @@ -0,0 +1,16 @@ + + + + + 0 0 1 0 0 0 + + + 0 0 3 0 0 0 + world + link + + 0 0 1 + + + + diff --git a/test/sdf/joint_screw_thread_pitch.sdf b/test/sdf/joint_screw_thread_pitch.sdf new file mode 100644 index 000000000..c3f22271c --- /dev/null +++ b/test/sdf/joint_screw_thread_pitch.sdf @@ -0,0 +1,52 @@ + + + + + 0 0 1 0 0 0 + + + 0 0 10 0 0 0 + + + + 0 0 1 + + child_link + parent_link + + + + 0 0 1 + + child_link + parent_link + 0.5 + -12.566 + + + + 0 0 1 + + child_link + parent_link + 0.5 + + + + 0 0 1 + + child_link + parent_link + -12.566 + + + + 0 0 1 + + child_link + parent_link + 0.5 + 0.5 + + + diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index e7dc5f959..219dae933 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -25,6 +25,7 @@ false 0.1 0.2 0.3 0 0 0 + /camera_sensor/camera_info .75 true my_camera/trigger diff --git a/test/sdf/world_joint_child_frame.sdf b/test/sdf/world_joint_child_frame.sdf new file mode 100644 index 000000000..9d0bc95b9 --- /dev/null +++ b/test/sdf/world_joint_child_frame.sdf @@ -0,0 +1,32 @@ + + + + + + 0 0 1 0 0 0 + + + + 0 0 10 0 0 0 + + + + 0 1 0 0 0 0 + + + 1 0 0 0 0 0 + parent_model::L + child_frame + + + 10 0 0 0 0 0 + parent_model::L + child_model::L + + + diff --git a/test/sdf/world_joint_invalid_child_world.sdf b/test/sdf/world_joint_invalid_child_world.sdf new file mode 100644 index 000000000..6502eabc2 --- /dev/null +++ b/test/sdf/world_joint_invalid_child_world.sdf @@ -0,0 +1,28 @@ + + + + + + 0 0 1 0 0 0 + + + + 0 1 0 0 0 0 + + + 1 0 0 0 0 0 + parent_model::L + child_frame + + + 10 0 0 0 0 0 + parent_model::L + world + + + diff --git a/test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf b/test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf new file mode 100644 index 000000000..aabdd0ad4 --- /dev/null +++ b/test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf @@ -0,0 +1,27 @@ + + + + + + 0 0 1 0 0 0 + + + + 0 0 10 0 0 0 + + + + 0 1 0 0 0 0 + + + 10 0 0 0 0 0 + child_model + child_frame + + + diff --git a/test/sdf/world_joint_parent_frame.sdf b/test/sdf/world_joint_parent_frame.sdf new file mode 100644 index 000000000..6418bd8cc --- /dev/null +++ b/test/sdf/world_joint_parent_frame.sdf @@ -0,0 +1,32 @@ + + + + + + 0 0 1 0 0 0 + + + + 0 0 10 0 0 0 + + + + 0 1 0 0 0 0 + + + 1 0 0 0 0 0 + parent_frame + child_model::L + + + 10 0 0 0 0 0 + parent_model::L + child_model::L + + +