diff --git a/CMakeLists.txt b/CMakeLists.txt index b11259156..9fd7643b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,14 +22,19 @@ find_package(gz-cmake3 REQUIRED) set(GZ_CMAKE_VER ${gz-cmake3_VERSION_MAJOR}) ######################################## +option(SKIP_PYBIND11 + "Skip generating Python bindings via pybind11" + OFF) + # Python interfaces vars -option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION +include(CMakeDependentOption) +cmake_dependent_option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION "Install python modules in standard system paths in the system" - OFF) + OFF "NOT SKIP_PYBIND11" OFF) -option(USE_DIST_PACKAGES_FOR_PYTHON +cmake_dependent_option(USE_DIST_PACKAGES_FOR_PYTHON "Use dist-packages instead of site-package to install python modules" - OFF) + OFF "NOT SKIP_PYBIND11" OFF) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -38,7 +43,7 @@ if (BUILD_SDF) gz_configure_project( NO_PROJECT_PREFIX REPLACE_INCLUDE_PATH sdf - VERSION_SUFFIX pre1) + VERSION_SUFFIX) ################################################# # Find tinyxml2. @@ -133,16 +138,20 @@ if (BUILD_SDF) else() message (STATUS "Searching for Python - found version ${Python3_VERSION}.") - set(PYBIND11_PYTHON_VERSION 3) - find_package(pybind11 2.4 QUIET) + if (SKIP_PYBIND11) + message(STATUS "SKIP_PYBIND11 set - disabling python bindings") + else() + set(PYBIND11_PYTHON_VERSION 3) + find_package(pybind11 2.4 QUIET) - if (${pybind11_FOUND}) - find_package(Python3 ${GZ_PYTHON_VERSION} REQUIRED COMPONENTS Development) - message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") - else() - GZ_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") - message (STATUS "Searching for pybind11 - not found.") - endif() + if (${pybind11_FOUND}) + find_package(Python3 ${GZ_PYTHON_VERSION} REQUIRED COMPONENTS Development) + message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") + else() + GZ_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") + message (STATUS "Searching for pybind11 - not found.") + endif() + endif() endif() gz_configure_build(HIDE_SYMBOLS_BY_DEFAULT QUIT_IF_BUILD_ERRORS) @@ -152,7 +161,7 @@ if (BUILD_SDF) add_subdirectory(sdf) add_subdirectory(conf) add_subdirectory(doc) - if (${pybind11_FOUND}) + if (pybind11_FOUND AND NOT SKIP_PYBIND11) add_subdirectory(python) endif() endif(BUILD_SDF) diff --git a/Changelog.md b/Changelog.md index 5bf0537ac..43f88a3c6 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,11 +4,245 @@ ## libsdformat 13.X -### libsdformat 13.0.0 (202X-XX-XX) +### libsdformat 13.0.1 (2022-09-27) + +1. Fix arm tests + * [Pull request #1173](https://github.com/gazebosim/sdformat/pull/1173) + +### libsdformat 13.0.0 (2022-09-23) + +1. Add camera `optical_frame_id` element + * [Pull request #1109](https://github.com/gazebosim/sdformat/pull/1109) + +1. sdf/1.10: remove unused spec files + * [Pull request #1113](https://github.com/gazebosim/sdformat/pull/1113) + +1. 1.10/joint.sdf: better default limit values + * [Pull request #1112](https://github.com/gazebosim/sdformat/pull/1112) + +1. Remove unused macros from config.hh + * [Pull request #1108](https://github.com/gazebosim/sdformat/pull/1108) + +1. Make //plugin/@name optional + * [Pull request #1101](https://github.com/gazebosim/sdformat/pull/1101) + +1. Add Error enums and update Migration guide + * [Pull request #1099](https://github.com/gazebosim/sdformat/pull/1099) + +1. Warn by default on unrecognized elements + * [Pull request #1096](https://github.com/gazebosim/sdformat/pull/1096) + +1. InterfaceElements: remove deprecated data members + * [Pull request #1097](https://github.com/gazebosim/sdformat/pull/1097) + +1. Add fluid added mass to inertial + * [Pull request #1077](https://github.com/gazebosim/sdformat/pull/1077) + +1. Change Root to non-unique impl pointer + * [Pull request #844](https://github.com/gazebosim/sdformat/pull/844) + +1. Add python interfaces + * [Pull request #932](https://github.com/gazebosim/sdformat/pull/932) + * [Pull request #933](https://github.com/gazebosim/sdformat/pull/933) + * [Pull request #934](https://github.com/gazebosim/sdformat/pull/934) + * [Pull request #937](https://github.com/gazebosim/sdformat/pull/937) + * [Pull request #938](https://github.com/gazebosim/sdformat/pull/938) + * [Pull request #940](https://github.com/gazebosim/sdformat/pull/940) + * [Pull request #941](https://github.com/gazebosim/sdformat/pull/941) + * [Pull request #942](https://github.com/gazebosim/sdformat/pull/942) + * [Pull request #944](https://github.com/gazebosim/sdformat/pull/944) + * [Pull request #945](https://github.com/gazebosim/sdformat/pull/945) + * [Pull request #946](https://github.com/gazebosim/sdformat/pull/946) + * [Pull request #947](https://github.com/gazebosim/sdformat/pull/947) + * [Pull request #948](https://github.com/gazebosim/sdformat/pull/948) + * [Pull request #949](https://github.com/gazebosim/sdformat/pull/949) + * [Pull request #950](https://github.com/gazebosim/sdformat/pull/950) + * [Pull request #951](https://github.com/gazebosim/sdformat/pull/951) + * [Pull request #952](https://github.com/gazebosim/sdformat/pull/952) + * [Pull request #953](https://github.com/gazebosim/sdformat/pull/953) + * [Pull request #957](https://github.com/gazebosim/sdformat/pull/957) + * [Pull request #960](https://github.com/gazebosim/sdformat/pull/960) + * [Pull request #961](https://github.com/gazebosim/sdformat/pull/961) + * [Pull request #962](https://github.com/gazebosim/sdformat/pull/962) + * [Pull request #963](https://github.com/gazebosim/sdformat/pull/963) + * [Pull request #964](https://github.com/gazebosim/sdformat/pull/964) + * [Pull request #967](https://github.com/gazebosim/sdformat/pull/967) + * [Pull request #968](https://github.com/gazebosim/sdformat/pull/968) + * [Pull request #969](https://github.com/gazebosim/sdformat/pull/969) + * [Pull request #970](https://github.com/gazebosim/sdformat/pull/970) + * [Pull request #971](https://github.com/gazebosim/sdformat/pull/971) + * [Pull request #972](https://github.com/gazebosim/sdformat/pull/972) + * [Pull request #973](https://github.com/gazebosim/sdformat/pull/973) + * [Pull request #981](https://github.com/gazebosim/sdformat/pull/981) + * [Pull request #982](https://github.com/gazebosim/sdformat/pull/982) + * [Pull request #983](https://github.com/gazebosim/sdformat/pull/983) + * [Pull request #984](https://github.com/gazebosim/sdformat/pull/984) + * [Pull request #988](https://github.com/gazebosim/sdformat/pull/988) + * [Pull request #989](https://github.com/gazebosim/sdformat/pull/989) + * [Pull request #992](https://github.com/gazebosim/sdformat/pull/992) + * [Pull request #993](https://github.com/gazebosim/sdformat/pull/993) + * [Pull request #994](https://github.com/gazebosim/sdformat/pull/994) + * [Pull request #995](https://github.com/gazebosim/sdformat/pull/995) + * [Pull request #996](https://github.com/gazebosim/sdformat/pull/996) + * [Pull request #997](https://github.com/gazebosim/sdformat/pull/997) + * [Pull request #998](https://github.com/gazebosim/sdformat/pull/998) + * [Pull request #999](https://github.com/gazebosim/sdformat/pull/999) + * [Pull request #1001](https://github.com/gazebosim/sdformat/pull/1001) + * [Pull request #1020](https://github.com/gazebosim/sdformat/pull/1020) + * [Pull request #1028](https://github.com/gazebosim/sdformat/pull/1028) + * [Pull request #1029](https://github.com/gazebosim/sdformat/pull/1029) + * [Pull request #1036](https://github.com/gazebosim/sdformat/pull/1036) + * [Pull request #1060](https://github.com/gazebosim/sdformat/pull/1060) + * [Pull request #1061](https://github.com/gazebosim/sdformat/pull/1061) + * [Pull request #1063](https://github.com/gazebosim/sdformat/pull/1063) + * [Pull request #1078](https://github.com/gazebosim/sdformat/pull/1078) + * [Pull request #1083](https://github.com/gazebosim/sdformat/pull/1083) + * [Pull request #1084](https://github.com/gazebosim/sdformat/pull/1084) + * [Pull request #1085](https://github.com/gazebosim/sdformat/pull/1085) + * [Pull request #1106](https://github.com/gazebosim/sdformat/pull/1106) + * [Pull request #1127](https://github.com/gazebosim/sdformat/pull/1127) + * [Pull request #1143](https://github.com/gazebosim/sdformat/pull/1143) + +1. Copy skybox uri field to sdf/1.10/scene.sdf + * [Pull request #1082](https://github.com/gazebosim/sdformat/pull/1082) + +1. Accept moon and custom surfaces in world spherical coordinates + * [Pull request #1050](https://github.com/gazebosim/sdformat/pull/1050) + +1. Migrate ign -> gz + * [Pull request #1008](https://github.com/gazebosim/sdformat/pull/1008) + * [Pull request #1022](https://github.com/gazebosim/sdformat/pull/1022) + * [Pull request #1040](https://github.com/gazebosim/sdformat/pull/1040) + * [Pull request #1045](https://github.com/gazebosim/sdformat/pull/1045) + * [Pull request #1047](https://github.com/gazebosim/sdformat/pull/1047) + * [Pull request #1048](https://github.com/gazebosim/sdformat/pull/1048) + * [Pull request #1052](https://github.com/gazebosim/sdformat/pull/1052) + * [Pull request #1057](https://github.com/gazebosim/sdformat/pull/1057) + * [Pull request #1058](https://github.com/gazebosim/sdformat/pull/1058) + * [Pull request #1067](https://github.com/gazebosim/sdformat/pull/1067) + * [Pull request #1074](https://github.com/gazebosim/sdformat/pull/1074) + * [Pull request #1078](https://github.com/gazebosim/sdformat/pull/1078) + * [Pull request #1092](https://github.com/gazebosim/sdformat/pull/1092) + +1. Copy 1.9 spec to 1.10 + * [Pull request #1073](https://github.com/gazebosim/sdformat/pull/1073) + * [Pull request #1076](https://github.com/gazebosim/sdformat/pull/1076) + +1. Root: get the world name + * [Pull request #1027](https://github.com/gazebosim/sdformat/pull/1027) + +1. Add SDF::SetRoot and deprecate non-const SDF::Root + * [Pull request #1070](https://github.com/gazebosim/sdformat/pull/1070) + +1. Update GoogleTest to latest version + * [Pull request #1059](https://github.com/gazebosim/sdformat/pull/1059) + * [Pull request #1072](https://github.com/gazebosim/sdformat/pull/1072) + +1. Update return types for Plugin's Name and Filename + * [Pull request #1055](https://github.com/gazebosim/sdformat/pull/1055) + +1. Surface::ToElement: add //friction/ode/mu + * [Pull request #1049](https://github.com/gazebosim/sdformat/pull/1049) + +1. Joint: rename parent/child `*LinkName` APIs + * [Pull request #1053](https://github.com/gazebosim/sdformat/pull/1053) + * [Pull request #1103](https://github.com/gazebosim/sdformat/pull/1103) + +1. Deprecate sdf::Inertia class + * [Pull request #1019](https://github.com/gazebosim/sdformat/pull/1019) + +1. Don't include the gz/math.hh header from library code + * [Pull request #1043](https://github.com/gazebosim/sdformat/pull/1043) + +1. Use pose multiplication instead of subtraction + * [Pull request #1039](https://github.com/gazebosim/sdformat/pull/1039) + +1. Remove deprecation warnings + * [Pull request #1014](https://github.com/gazebosim/sdformat/pull/1014) + +1. Added light methods to Link, Root and World + * [Pull request #1013](https://github.com/gazebosim/sdformat/pull/1013) + +1. Add sdf::Error logging in sdf::Param + * [Pull request #939](https://github.com/gazebosim/sdformat/pull/939) + +1. Changes for replacing PythonInterp with Python3 + * [Pull request #907](https://github.com/gazebosim/sdformat/pull/907) + +1. Combine find_package(ignition-utils) calls + * [Pull request #966](https://github.com/gazebosim/sdformat/pull/966) + +1. Change default floating point precision to max + * [Pull request #872](https://github.com/gazebosim/sdformat/pull/872) + +1. Clean up compiler warnings + * [Pull request #882](https://github.com/gazebosim/sdformat/pull/882) + +1. Switch to utils version of env functions + * [Pull request #854](https://github.com/gazebosim/sdformat/pull/854) + +1. Updated tests for ign-math's ostream fix + * [Pull request #847](https://github.com/gazebosim/sdformat/pull/847) +1. Infrastructure + * [Pull request #803](https://github.com/gazebosim/sdformat/pull/803) + * [Pull request #805](https://github.com/gazebosim/sdformat/pull/805) + * [Pull request #878](https://github.com/gazebosim/sdformat/pull/878) + * [Pull request #980](https://github.com/gazebosim/sdformat/pull/980) + * [Pull request #974](https://github.com/gazebosim/sdformat/pull/974) + +1. Remove completely unused define + * [Pull request #758](https://github.com/gazebosim/sdformat/pull/758) ## libsdformat 12.X +### libsdformat 12.6.0 (2022-09-07) + +1. Reduce the number of include dirs for sdformat. + * [Pull request #1170](https://github.com/gazebosim/sdformat/pull/1170) + +1. Add camera `optical_frame_id` element + * [Pull request #1109](https://github.com/gazebosim/sdformat/pull/1109) + +1. urdf: fix sensor/light pose for links lumped by fixed joints + * [Pull request #1114](https://github.com/gazebosim/sdformat/pull/1114) + +1. Removed USD component from SDFormat and move to gz-usd + * [Pull request #1094](https://github.com/gazebosim/sdformat/pull/1094) + +1. Fix URDF fixed joint reduction of SDF joints + * [Pull request #1089](https://github.com/gazebosim/sdformat/pull/1089) + +1. Test model name as `placement_frame` + * [Pull request #1079](https://github.com/gazebosim/sdformat/pull/1079) + +1. Test using `__model__`, `world` in `@attached_to`, `@relative_to` + * [Pull request #1066](https://github.com/gazebosim/sdformat/pull/1066) + +1. Remove unused sdf.hh.in template + * [Pull request #1081](https://github.com/gazebosim/sdformat/pull/1081) + +1. Readme: Ignition -> Gazebo + * [Pull request #1080](https://github.com/gazebosim/sdformat/pull/1080) + +1. Document major and minor SDFormat version numbers + * [Pull request #1065](https://github.com/gazebosim/sdformat/pull/1065) + +1. Add skybox URI + * [Pull request #1037](https://github.com/gazebosim/sdformat/pull/1037) + +1. Bash completion for flags + * [Pull request #1042](https://github.com/gazebosim/sdformat/pull/1042) + +1. Fix bug with resolving poses for joint sensors. + * [Pull request #1033](https://github.com/gazebosim/sdformat/pull/1033) + +1. sdf::Joint: Mutable versions of SensorByName and SensorByIndex + * [Pull request #1031](https://github.com/gazebosim/sdformat/pull/1031) + +1. Add Link::ResolveInertial API + * [Pull request #1012](https://github.com/gazebosim/sdformat/pull/1012) + ### libsdformat 12.5.0 (2022-05-12) 1. Add visibility mask to Lidar / Ray sensor @@ -1020,7 +1254,32 @@ ## libsdformat 9.X -### libsdformat 9.X.X (202X-XX-XX) +### 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: fix sensor/light pose for links lumped by fixed joints + * [Pull request #1114](https://github.com/gazebosim/sdformat/pull/1114) + +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) + +1. Test using `__model__`, `world` in @attached_to, @relative_to + * [Pull request #1066](https://github.com/gazebosim/sdformat/pull/1066) + +1. Readme: Ignition -> Gazebo + * [Pull request #1080](https://github.com/gazebosim/sdformat/pull/1080) + +1. Document major and minor SDFormat version numbers + * [Pull request #1065](https://github.com/gazebosim/sdformat/pull/1065) + +1. Bash completion for flags + * [Pull request #1042](https://github.com/gazebosim/sdformat/pull/1042) + +1. Add Link::ResolveInertial API + * [Pull request #1012](https://github.com/gazebosim/sdformat/pull/1012) ### libsdformat 9.8.0 (2022-04-26) diff --git a/Migration.md b/Migration.md index 7a98ba21c..513746002 100644 --- a/Migration.md +++ b/Migration.md @@ -70,6 +70,11 @@ but with improved human-readability.. 1. USD component now is living in https://github.com/gazebosim/gz-usd as an independent package. +1. URDF parser now properly transforms poses for lights, projectors and sensors + from gazebo extension tags that are moved to a new link during fixed joint + reduction. + + [pull request 1114](https://github.com/osrf/sdformat/pull/1114) + ## libsdformat 11.x to 12.0 An error is now emitted instead of a warning for a file containing more than @@ -334,6 +339,20 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. + std::optional GetMaxValueAsString() const; + bool ValidateValue() const; +## libsdformat 9.8.0 to 9.9.0 + +### Additions + +1. **sdf/Camera.hh** + + Get/Set functions for Camera projection matrix parameters. + +### Modifications + +1. URDF parser now properly transforms poses for lights, projectors and sensors + from gazebo extension tags that are moved to a new link during fixed joint + reduction. + + [pull request 1114](https://github.com/osrf/sdformat/pull/1114) + ## libsdformat 9.4 to 9.5 ### Additions @@ -538,8 +557,20 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. ### Modifications +1. **joint.sdf**: axis limits default values have changed + + `//limit/lower`: `-inf` (formerly `-1e16`) + + `//limit/upper`: `inf` (formerly `1e16`) + + `//limit/velocity`: `inf` (formerly `-1`) + + `//limit/effort`: `inf` (formerly `-1`) + 1. **plugin.sdf**: name attribute is now optional with empty default value. +### Removals + +1. **collision_engine.sdf**: unused specification file is removed. + +1. **urdf.sdf**: unused specification file is removed. + ## SDFormat specification 1.8 to 1.9 ### Additions diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index b047edb6a..8fb5260be 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -348,6 +348,20 @@ namespace sdf /// \param[in] _frame The name of the pose relative-to frame. public: void SetPoseRelativeTo(const std::string &_frame); + /// \brief 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. + /// \return The name of the frame this camera uses in its camera_info topic. + public: const std::string OpticalFrameId() const; + + /// \brief Set the name of the coordinate frame relative to which this + /// object's camera_info is expressed. + /// \param[in] _frame The frame this camera uses in its camera_info topic. + public: void SetOpticalFrameId(const std::string &_frame); + /// \brief Get the lens type. This is the type of the lens mapping. /// Supported values are gnomonical, stereographic, equidistant, /// equisolid_angle, orthographic, custom. For gnomonical (perspective) @@ -433,38 +447,86 @@ namespace sdf /// \param[in] _size The lens environment texture size. public: void SetLensEnvironmentTextureSize(int _size); - /// \brief Get the lens X focal length in pixels. - /// \return The lens X focal length in pixels. + /// \brief Get the lens intrinsic matrix X focal length in pixels. + /// \return The lens X focal length in pixels for the intrinsic matrix. public: double LensIntrinsicsFx() const; - /// \brief Set the lens X focal length in pixels. - /// \param[in] _fx The lens X focal length in pixels. + /// \brief Set the lens intrinsic matrix X focal length in pixels. + /// \param[in] _fx The intrinsic matrix lens X focal length in pixels. public: void SetLensIntrinsicsFx(double _fx); - /// \brief Get the lens Y focal length in pixels. - /// \return The lens Y focal length in pixels. + /// \brief Get the lens intrinsic matrix Y focal length in pixels. + /// \return The lens intrinsic matrix Y focal length in pixels. public: double LensIntrinsicsFy() const; - /// \brief Set the lens Y focal length in pixels. - /// \param[in] _fy The lens Y focal length in pixels. + /// \brief Set the lens intrinsic matrix Y focal length in pixels. + /// \param[in] _fy The lens intrinsic matrix Y focal length in pixels. public: void SetLensIntrinsicsFy(double _fy); - /// \brief Get the lens X principal point in pixels. - /// \return The lens X principal point in pixels. + /// \brief Get the lens intrinsic matrix X principal point in pixels. + /// \return The lens intrinsic matrix X principal point in pixels. public: double LensIntrinsicsCx() const; - /// \brief Set the lens X principal point in pixels. - /// \param[in] _cx The lens X principal point in pixels. + /// \brief Set the lens intrinsic matrix X principal point in pixels. + /// \param[in] _cx The lens intrinsic matrix X principal point in pixels. public: void SetLensIntrinsicsCx(double _cx); - /// \brief Get the lens Y principal point in pixels. - /// \return The lens Y principal point in pixels. + /// \brief Get the lens intrinsic matrix Y principal point in pixels. + /// \return The lens intrinsic matrix Y principal point in pixels. public: double LensIntrinsicsCy() const; - /// \brief Set the lens Y principal point in pixels. - /// \param[in] _cy The lens Y principal point in pixels. + /// \brief Set the lens intrinsic matrix Y principal point in pixels. + /// \param[in] _cy The lens intrinsic matrix Y principal point in pixels. public: void SetLensIntrinsicsCy(double _cy); + /// \brief Get the lens projection matrix X focal length in pixels. + /// \return The lens projection matrix X focal length in pixels. + public: double LensProjectionFx() const; + + /// \brief Set the lens projection matrix X focal length in pixels. + /// \param[in] _fx_p The lens projection matrix X focal length in pixels. + public: void SetLensProjectionFx(double _fx_p); + + /// \brief Get the lens projection matrix Y focal length in pixels. + /// \return The lens projection matrix Y focal length in pixels. + public: double LensProjectionFy() const; + + /// \brief Set the lens projection matrix Y focal length in pixels. + /// \param[in] _fy_p The lens projection matrix Y focal length in pixels. + public: void SetLensProjectionFy(double _fy_p); + + /// \brief Get the lens projection matrix X principal point in pixels. + /// \return The lens projection matrix X principal point in pixels. + public: double LensProjectionCx() const; + + /// \brief Set the lens projection matrix X principal point in pixels. + /// \param[in] _cx_p The lens projection matrix X principal point in pixels. + public: void SetLensProjectionCx(double _cx_p); + + /// \brief Get the lens projection matrix Y principal point in pixels. + /// \return The lens projection matrix Y principal point in pixels. + public: double LensProjectionCy() const; + + /// \brief Set the lens projection matrix Y principal point in pixels. + /// \param[in] _cy_p The lens projection matrix Y principal point in pixels. + public: void SetLensProjectionCy(double _cy_p); + + /// \brief Get the lens projection matrix X translation in pixels. + /// \return The lens projection matrix X translation in pixels. + public: double LensProjectionTx() const; + + /// \brief Set the lens projection matrix X translation in pixels. + /// \param[in] _tx The lens projection matrix X translation in pixels. + public: void SetLensProjectionTx(double _tx); + + /// \brief Get the lens projection matrix Y translation in pixels. + /// \return The lens projection matrix Y translation in pixels. + public: double LensProjectionTy() const; + + /// \brief Set the lens projection matrix Y translation in pixels. + /// \param[in] _ty The lens projection matrix Y translation in pixels. + public: void SetLensProjectionTy(double _ty); + /// \brief Get the lens XY axis skew. /// \return The lens XY axis skew. public: double LensIntrinsicsSkew() const; diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 76ee5ce23..26b5dd843 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -34,6 +34,7 @@ namespace sdf // // Forward declaration. class Geometry; + class ParserConfig; class Surface; struct PoseRelativeToGraph; template class ScopedGraph; @@ -55,6 +56,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the collision based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the name of the collision. /// The name of the collision must be unique within the scope of a Link. /// \return Name of the collision. diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 55d893e14..5ff016f97 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -37,6 +37,7 @@ namespace sdf class Ellipsoid; class Heightmap; class Mesh; + class ParserConfig; class Plane; class Polyline; class Sphere; @@ -93,6 +94,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the geometry based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the type of geometry. /// \return The geometry type. public: GeometryType Type() const; diff --git a/include/sdf/Heightmap.hh b/include/sdf/Heightmap.hh index 2071a057d..abba9e443 100644 --- a/include/sdf/Heightmap.hh +++ b/include/sdf/Heightmap.hh @@ -28,6 +28,11 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { + // + + // Forward declarations. + class ParserConfig; + /// \brief Texture to be used on heightmaps. class SDFORMAT_VISIBLE HeightmapTexture { @@ -42,6 +47,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the heightmap texture geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the heightmap texture's size. /// \return The size of the heightmap texture in meters. public: double Size() const; @@ -129,6 +143,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the heightmap geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the heightmap's URI. /// \return The URI of the heightmap data. public: std::string Uri() const; diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index f92824d7c..a5b3c4de7 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -37,6 +37,7 @@ namespace sdf // Forward declarations. class Collision; class Light; + class ParserConfig; class ParticleEmitter; class Sensor; class Visual; @@ -56,6 +57,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the link based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the name of the link. /// The name of a link must be unique within the scope of a Model. /// \return Name of the link. diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index 5d076f0cc..ae3e6339e 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -31,6 +31,7 @@ namespace sdf // // Forward declarations. + class ParserConfig; class Pbr; enum class ShaderType : int @@ -55,6 +56,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the material based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the ambient color. The ambient color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 1752e370d..c04e0f748 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -30,6 +30,9 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // + // Forward declarations. + class ParserConfig; + /// \brief Mesh represents a mesh shape, and is usually accessed through a /// Geometry. class SDFORMAT_VISIBLE Mesh @@ -45,6 +48,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the mesh geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the mesh's URI. /// \return The URI of the mesh data. public: std::string Uri() const; diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index de266f36f..29c5b90e5 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -682,7 +682,7 @@ namespace sdf try { std::stringstream ss; - ss << _value; + ss << ParamStreamer{_value, std::numeric_limits::max()}; return this->SetFromString(ss.str(), true, _errors); } catch(...) @@ -777,7 +777,8 @@ namespace sdf try { - ss << ParamStreamer{this->dataPtr->defaultValue}; + ss << ParamStreamer{this->dataPtr->defaultValue, + std::numeric_limits::max()}; ss >> _value; } catch(...) diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index 69c5cdf42..af17a8e76 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -169,11 +169,29 @@ class SDFORMAT_VISIBLE ParserConfig public: const std::vector &CustomModelParsers() const; /// \brief Set the preserveFixedJoint flag. + /// \param[in] _preserveFixedJoint flag value to set public: void URDFSetPreserveFixedJoint(bool _preserveFixedJoint); /// \brief Get the preserveFixedJoint flag value. + /// \return Current flag value public: bool URDFPreserveFixedJoint() const; + /// \brief Set the storeResolvedURIs flag value. + /// \param[in] _resolveURI True to make the parser attempt to resolve any + /// URIs found and store them. False to preserve original URIs + /// + /// The Parser will use the FindFileCallback provided to attempt to resolve + /// URIs in the Mesh, Material, Heightmap, and Skybox DOM objects + /// If the FindFileCallback provides a non-empty string, the URI will be + /// stored in the DOM object, and the original (unresolved) URI will be + /// stored in the underlying Element. + public: void SetStoreResovledURIs(bool _resolveURI); + + /// \brief Get the storeResolvedURIs flag value. + /// \return True if the parser will attempt to resolve any URIs found and + /// store them. False to preserve original URIs + public: bool StoreResolvedURIs() const; + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Scene.hh b/include/sdf/Scene.hh index 922073063..f192adff1 100644 --- a/include/sdf/Scene.hh +++ b/include/sdf/Scene.hh @@ -43,6 +43,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the scene based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the ambient color of the scene /// \return Scene ambient color public: gz::math::Color Ambient() const; diff --git a/include/sdf/Sky.hh b/include/sdf/Sky.hh index bf26a3498..f677a3c73 100644 --- a/include/sdf/Sky.hh +++ b/include/sdf/Sky.hh @@ -32,6 +32,11 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { + // + + // Forward declarations. + class ParserConfig; + class SDFORMAT_VISIBLE Sky { /// \brief Default constructor @@ -117,6 +122,14 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the sky based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf, const ParserConfig &_config); + /// \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 diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index bc1b8898c..ff9da957f 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -41,6 +41,7 @@ namespace sdf // Forward declarations. class Geometry; + class ParserConfig; struct PoseRelativeToGraph; template class ScopedGraph; @@ -57,6 +58,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the visual based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the name of the visual. /// The name of the visual must be unique within the scope of a Link. /// \return Name of the visual. diff --git a/include/sdf/config.hh.in b/include/sdf/config.hh.in index 4d5612030..d1b3db532 100644 --- a/include/sdf/config.hh.in +++ b/include/sdf/config.hh.in @@ -45,8 +45,6 @@ #define SDF_VERSION_HEADER "Simulation Description Format (SDF), version ${SDF_PROTOCOL_VERSION}\nCopyright (C) 2014 Open Source Robotics Foundation.\nReleased under the Apache 2 License.\nhttp://gazebosim.org/sdf\n\n" -#cmakedefine HAVE_URDFDOM 1 -#cmakedefine USE_INTERNAL_URDF 1 #cmakedefine SDFORMAT_DISABLE_CONSOLE_LOGFILE 1 #define SDF_SHARE_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/" diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b84023c92..e536c749f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -36,7 +36,10 @@ function(configure_build_install_location _library_name) ) endfunction() -pybind11_add_module(sdformat SHARED +# sdformatX target already exists, use pysdformatX + OUTPUT_NAME to get +# sdformatX file name generated and map BINDINGS_MODULE_NAME to sdformatX +set(BINDINGS_MODULE_NAME "pysdformat${PROJECT_VERSION_MAJOR}") +pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/sdf/_gz_sdformat_pybind11.cc src/sdf/pyAirPressure.cc src/sdf/pyAltimeter.cc @@ -84,11 +87,19 @@ pybind11_add_module(sdformat SHARED src/sdf/pybind11_helpers.cc ) -target_link_libraries(sdformat PRIVATE +target_link_libraries(${BINDINGS_MODULE_NAME} PRIVATE ${PROJECT_LIBRARY_TARGET_NAME} ) -configure_build_install_location(sdformat) +# different from the target name since the target name was not able to use +# sdformatX since it conflicts with the project name +target_compile_definitions(${BINDINGS_MODULE_NAME} PRIVATE + BINDINGS_MODULE_NAME=sdformat${PROJECT_VERSION_MAJOR}) + +set_target_properties(${BINDINGS_MODULE_NAME} PROPERTIES + OUTPUT_NAME "sdformat${PROJECT_VERSION_MAJOR}") + +configure_build_install_location(${BINDINGS_MODULE_NAME}) if (BUILD_TESTING) pybind11_add_module(sdformattest SHARED diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 294819cde..0e64d1e8a 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -16,6 +16,7 @@ #include #include +#include #include "pyAirPressure.hh" #include "pyAltimeter.hh" @@ -62,9 +63,15 @@ #include "pyVisual.hh" #include "pyWorld.hh" -PYBIND11_MODULE(sdformat, m) { +PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { m.doc() = "sdformat Python Library."; + // Import the gz.math library to automatically add the type conversions + // this module requires to pass mathematical types to python code. + std::string gzMathModule = + std::string("gz.math") + std::to_string(GZ_MATH_MAJOR_VERSION); + pybind11::module::import(gzMathModule.c_str()); + sdf::python::defineAirPressure(m); sdf::python::defineAltimeter(m); sdf::python::defineAtmosphere(m); diff --git a/python/test/gz_test_deps/README b/python/test/gz_test_deps/README new file mode 100644 index 000000000..29f5c57e1 --- /dev/null +++ b/python/test/gz_test_deps/README @@ -0,0 +1,8 @@ +This python package encapsulates versioned python packages of gz library +bindings such that the version number only has to be changed in this package. +Here's an example of how to use this in a python test: + +```python +from gz_test_deps import math # instead of from gz import math7 +from gz_test_deps.math import Vector3d # instead of from gz.math7 import Vector3d +``` diff --git a/python/test/gz_test_deps/__init__.py b/python/test/gz_test_deps/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/python/test/gz_test_deps/math.py b/python/test/gz_test_deps/math.py new file mode 100644 index 000000000..cb2860c79 --- /dev/null +++ b/python/test/gz_test_deps/math.py @@ -0,0 +1 @@ +from gz.math7 import * diff --git a/python/test/gz_test_deps/sdformat.py b/python/test/gz_test_deps/sdformat.py new file mode 100644 index 000000000..51cec2889 --- /dev/null +++ b/python/test/gz_test_deps/sdformat.py @@ -0,0 +1 @@ +from sdformat14 import * diff --git a/python/test/pyAirPressure_TEST.py b/python/test/pyAirPressure_TEST.py index 0b294ade5..254646786 100644 --- a/python/test/pyAirPressure_TEST.py +++ b/python/test/pyAirPressure_TEST.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math import Vector3d -from sdformat import AirPressure, Noise -import sdformat as sdf +from gz_test_deps.sdformat import AirPressure, Noise +import gz_test_deps.sdformat as sdf import unittest class AtmosphereTEST(unittest.TestCase): diff --git a/python/test/pyAltimeter_TEST.py b/python/test/pyAltimeter_TEST.py index ff9b9498c..8a5fbad85 100644 --- a/python/test/pyAltimeter_TEST.py +++ b/python/test/pyAltimeter_TEST.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math import Vector3d -from sdformat import Altimeter, Noise -import sdformat as sdf +from gz_test_deps.sdformat import Altimeter, Noise +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyAtmosphere_TEST.py b/python/test/pyAtmosphere_TEST.py index e5162159b..cffc26ee6 100644 --- a/python/test/pyAtmosphere_TEST.py +++ b/python/test/pyAtmosphere_TEST.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math import Pose3d, Temperature -from sdformat import Atmosphere -import sdformat as sdf +from gz_test_deps.math import Temperature +from gz_test_deps.sdformat import Atmosphere +import gz_test_deps.sdformat as sdf import unittest class AtmosphereTEST(unittest.TestCase): diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index 4eac4f2b7..d98440de5 100644 --- a/python/test/pyBox_TEST.py +++ b/python/test/pyBox_TEST.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math import Vector3d -from sdformat import Box +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import Box import unittest class BoxTEST(unittest.TestCase): diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index e3ec95261..40000b7f0 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -13,10 +13,10 @@ # limitations under the License. import copy -from gz.math import Angle, Pose3d, Vector2d +from gz_test_deps.math import Angle, Pose3d, Vector2d import math -from sdformat import Camera -import sdformat as sdf +from gz_test_deps.sdformat import Camera +import gz_test_deps.sdformat as sdf import unittest class CameraTEST(unittest.TestCase): diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index 1e375dd6e..52472ed91 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -14,11 +14,9 @@ import copy -from gz.math import Vector3d, Capsuled - import math -from sdformat import Capsule +from gz_test_deps.sdformat import Capsule import unittest diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index fbb6d5d82..9e92f521b 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -13,10 +13,11 @@ # limitations under the License. import copy -from gz.math import Pose3d -from sdformat import (Box, Collision, Contact, Cylinder, Error, Geometry, - Plane, Surface, Sphere, SDFErrorsException) -import sdformat as sdf +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error, + Geometry, Plane, Surface, Sphere, + SDFErrorsException) +import gz_test_deps.sdformat as sdf import unittest import math diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index 7294abb54..99bcf7823 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -14,11 +14,9 @@ import copy -from gz.math import Vector3d - import math -from sdformat import Cylinder +from gz_test_deps.sdformat import Cylinder import unittest diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index c77105f7d..a102a42ad 100644 --- a/python/test/pyEllipsoid_TEST.py +++ b/python/test/pyEllipsoid_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz.math import Vector3d, Ellipsoidd +from gz_test_deps.math import Vector3d import math -from sdformat import Ellipsoid +from gz_test_deps.sdformat import Ellipsoid import unittest diff --git a/python/test/pyError_TEST.py b/python/test/pyError_TEST.py index b8c82e595..f9b075ac1 100644 --- a/python/test/pyError_TEST.py +++ b/python/test/pyError_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import Error -import sdformat as sdf +from gz_test_deps.sdformat import Error +import gz_test_deps.sdformat as sdf import unittest class ErrorColor(unittest.TestCase): diff --git a/python/test/pyForceTorque_TEST.py b/python/test/pyForceTorque_TEST.py index 7a72bff90..3ca3a8d8f 100644 --- a/python/test/pyForceTorque_TEST.py +++ b/python/test/pyForceTorque_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import ForceTorque, Noise -import sdformat as sdf +from gz_test_deps.sdformat import ForceTorque, Noise +import gz_test_deps.sdformat as sdf import unittest class ForceTorqueTEST(unittest.TestCase): diff --git a/python/test/pyFrame_TEST.py b/python/test/pyFrame_TEST.py index bf0dcb9e7..fb93e4d3f 100644 --- a/python/test/pyFrame_TEST.py +++ b/python/test/pyFrame_TEST.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import copy -from gz.math import Pose3d -from sdformat import Frame, Error, SDFErrorsException, ErrorCode +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import Frame, Error, SDFErrorsException, ErrorCode import unittest import math diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index dcee570fb..a1f4ec65b 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -13,9 +13,10 @@ # limitations under the License. import copy -from sdformat import Geometry, Box, Capsule, Cylinder, Ellipsoid, Mesh, Plane, Sphere -from gz.math import Vector3d, Vector2d -import sdformat as sdf +from gz_test_deps.sdformat import (Geometry, Box, Capsule, Cylinder, Ellipsoid, + Mesh, Plane, Sphere) +from gz_test_deps.math import Vector3d, Vector2d +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyGui_TEST.py b/python/test/pyGui_TEST.py index 7c66ca4c4..f7581b749 100644 --- a/python/test/pyGui_TEST.py +++ b/python/test/pyGui_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import Gui, Plugin +from gz_test_deps.sdformat import Gui, Plugin import unittest diff --git a/python/test/pyHeightmap_TEST.py b/python/test/pyHeightmap_TEST.py index d58bfa7ea..13848ad5f 100644 --- a/python/test/pyHeightmap_TEST.py +++ b/python/test/pyHeightmap_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math import Vector3d -from sdformat import Heightmap, HeightmapBlend, HeightmapTexture +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import Heightmap, HeightmapBlend, HeightmapTexture import unittest diff --git a/python/test/pyIMU_TEST.py b/python/test/pyIMU_TEST.py index 8eda014ec..d1bf66ebd 100644 --- a/python/test/pyIMU_TEST.py +++ b/python/test/pyIMU_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz.math import Vector3d -from sdformat import IMU, Noise -import sdformat as sdf +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import IMU, Noise +import gz_test_deps.sdformat as sdf import unittest class IMUTest(unittest.TestCase): diff --git a/python/test/pyJointAxis_TEST.py b/python/test/pyJointAxis_TEST.py index d9ac24aab..4a1dffafa 100644 --- a/python/test/pyJointAxis_TEST.py +++ b/python/test/pyJointAxis_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math import Pose3d, Vector3d -from sdformat import JointAxis, Error, SDFErrorsException +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import JointAxis, Error, SDFErrorsException import math import unittest @@ -29,8 +29,8 @@ def test_default_construction(self): self.assertAlmostEqual(0.0, axis.friction()) self.assertAlmostEqual(0.0, axis.spring_reference()) self.assertAlmostEqual(0.0, axis.spring_stiffness()) - self.assertAlmostEqual(-1e16, axis.lower()) - self.assertAlmostEqual(1e16, axis.upper()) + self.assertAlmostEqual(-math.inf, axis.lower()) + self.assertAlmostEqual(math.inf, axis.upper()) self.assertAlmostEqual(math.inf, axis.effort()) self.assertAlmostEqual(math.inf, axis.max_velocity()) self.assertAlmostEqual(1e8, axis.stiffness()) diff --git a/python/test/pyJoint_TEST.py b/python/test/pyJoint_TEST.py index f0ad371a9..f8577b93c 100644 --- a/python/test/pyJoint_TEST.py +++ b/python/test/pyJoint_TEST.py @@ -13,10 +13,10 @@ # limitations under the License. import copy -from gz.math import Pose3d, Vector3d -from sdformat import (Joint, JointAxis, Error, SemanticPose, Sensor, - SDFErrorsException) -import sdformat as sdf +from gz_test_deps.math import Pose3d, Vector3d +from gz_test_deps.sdformat import (Joint, JointAxis, Error, SemanticPose, + Sensor, SDFErrorsException) +import gz_test_deps.sdformat as sdf import math import unittest diff --git a/python/test/pyLidar_TEST.py b/python/test/pyLidar_TEST.py index b9ea1b7bb..2f781948e 100644 --- a/python/test/pyLidar_TEST.py +++ b/python/test/pyLidar_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math import Angle, Pose3d, Vector3d, Helpers -from sdformat import Lidar, Error, Noise +from gz_test_deps.math import Angle, Helpers +from gz_test_deps.sdformat import Lidar, Error, Noise import math import unittest diff --git a/python/test/pyLight_TEST.py b/python/test/pyLight_TEST.py index 167757e4c..233804270 100644 --- a/python/test/pyLight_TEST.py +++ b/python/test/pyLight_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz.math import Angle, Color, Pose3d, Vector3d -from sdformat import Light, SDFErrorsException -import sdformat as sdf +from gz_test_deps.math import Angle, Color, Pose3d, Vector3d +from gz_test_deps.sdformat import Light, SDFErrorsException +import gz_test_deps.sdformat as sdf import math import unittest diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index 4bb10665c..7bf0a3494 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz.math import Pose3d, Inertiald, MassMatrix3d, Vector3d -from sdformat import (Collision, Light, Link, Sensor, Visual, - SDFErrorsException) +from gz_test_deps.math import Pose3d, Inertiald, MassMatrix3d, Vector3d +from gz_test_deps.sdformat import (Collision, Light, Link, Sensor, Visual, + SDFErrorsException) import unittest import math diff --git a/python/test/pyMagnetometer_TEST.py b/python/test/pyMagnetometer_TEST.py index 6a7597346..a875628dc 100644 --- a/python/test/pyMagnetometer_TEST.py +++ b/python/test/pyMagnetometer_TEST.py @@ -13,9 +13,8 @@ # limitations under the License. import copy -from gz.math import Pose3d -from sdformat import Magnetometer, Noise -import sdformat as sdf +from gz_test_deps.sdformat import Magnetometer, Noise +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyMaterial_TEST.py b/python/test/pyMaterial_TEST.py index ec180df6d..01198aab0 100644 --- a/python/test/pyMaterial_TEST.py +++ b/python/test/pyMaterial_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from sdformat import Material, Pbr, PbrWorkflow -from gz.math import Color -import sdformat as sdf +from gz_test_deps.sdformat import Material, Pbr, PbrWorkflow +from gz_test_deps.math import Color +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index a854d5c5f..64d89a57a 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import Mesh -from gz.math import Vector3d +from gz_test_deps.sdformat import Mesh +from gz_test_deps.math import Vector3d import unittest diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py index efb0aa303..ce8d7e5b4 100644 --- a/python/test/pyModel_TEST.py +++ b/python/test/pyModel_TEST.py @@ -13,10 +13,10 @@ # limitations under the License. import copy -from gz.math import Pose3d, Vector3d -from sdformat import (Plugin, Model, Joint, Link, Error, Frame, SemanticPose, - SDFErrorsException) -import sdformat as sdf +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import (Plugin, Model, Joint, Link, Error, Frame, + SemanticPose, SDFErrorsException) +import gz_test_deps.sdformat as sdf import math import unittest diff --git a/python/test/pyNavSat_TEST.py b/python/test/pyNavSat_TEST.py index e6383f06a..783828ab9 100644 --- a/python/test/pyNavSat_TEST.py +++ b/python/test/pyNavSat_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import NavSat, Noise +from gz_test_deps.sdformat import NavSat, Noise import unittest class NavSatColor(unittest.TestCase): diff --git a/python/test/pyNoise_TEST.py b/python/test/pyNoise_TEST.py index f8e47e61e..0e444cde3 100644 --- a/python/test/pyNoise_TEST.py +++ b/python/test/pyNoise_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import Noise -import sdformat as sdf +from gz_test_deps.sdformat import Noise +import gz_test_deps.sdformat as sdf import math import unittest diff --git a/python/test/pyParserConfig_TEST.py b/python/test/pyParserConfig_TEST.py index d2516d98c..a56bfc405 100644 --- a/python/test/pyParserConfig_TEST.py +++ b/python/test/pyParserConfig_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import ParserConfig +from gz_test_deps.sdformat import ParserConfig from sdformattest import source_file, test_file import unittest diff --git a/python/test/pyParticleEmitter_TEST.py b/python/test/pyParticleEmitter_TEST.py index 2464bb1b2..9995eb66d 100644 --- a/python/test/pyParticleEmitter_TEST.py +++ b/python/test/pyParticleEmitter_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math import Color, Pose3d, Vector3d, Helpers -from sdformat import ParticleEmitter +from gz_test_deps.math import Color, Pose3d, Vector3d, Helpers +from gz_test_deps.sdformat import ParticleEmitter import unittest diff --git a/python/test/pyPbr_TEST.py b/python/test/pyPbr_TEST.py index 2faf52164..3ab554e5f 100644 --- a/python/test/pyPbr_TEST.py +++ b/python/test/pyPbr_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import Pbr, PbrWorkflow -import sdformat as sdf +from gz_test_deps.sdformat import Pbr, PbrWorkflow +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyPhysics_TEST.py b/python/test/pyPhysics_TEST.py index d053a9d33..f2425ff52 100644 --- a/python/test/pyPhysics_TEST.py +++ b/python/test/pyPhysics_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import Physics +from gz_test_deps.sdformat import Physics import unittest diff --git a/python/test/pyPlane_TEST.py b/python/test/pyPlane_TEST.py index bd0729f02..ad2536b5d 100644 --- a/python/test/pyPlane_TEST.py +++ b/python/test/pyPlane_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import Plane -from gz.math import Vector3d, Vector2d, Planed +from gz_test_deps.sdformat import Plane +from gz_test_deps.math import Vector3d, Vector2d import unittest diff --git a/python/test/pyPlugin_TEST.py b/python/test/pyPlugin_TEST.py index 058a7af87..2db8ceedd 100644 --- a/python/test/pyPlugin_TEST.py +++ b/python/test/pyPlugin_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import Plugin +from gz_test_deps.sdformat import Plugin import unittest diff --git a/python/test/pyPolyline_TEST.py b/python/test/pyPolyline_TEST.py index 2b544e190..ea93acecf 100644 --- a/python/test/pyPolyline_TEST.py +++ b/python/test/pyPolyline_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import Polyline -from gz.math import Vector2d +from gz_test_deps.sdformat import Polyline +from gz_test_deps.math import Vector2d import unittest diff --git a/python/test/pyRoot_TEST.py b/python/test/pyRoot_TEST.py index 3bd90e9e1..0df1d3023 100644 --- a/python/test/pyRoot_TEST.py +++ b/python/test/pyRoot_TEST.py @@ -13,10 +13,11 @@ # limitations under the License. import copy -from gz.math import Vector3d, Pose3d -from sdformat import (Error, Model, Light, Root, SDF_VERSION, - SDFErrorsException, SDF_PROTOCOL_VERSION, World) -import sdformat as sdf +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import (Error, Model, Light, Root, SDF_VERSION, + SDFErrorsException, SDF_PROTOCOL_VERSION, + World) +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyScene_TEST.py b/python/test/pyScene_TEST.py index 294149075..6f3448c58 100644 --- a/python/test/pyScene_TEST.py +++ b/python/test/pyScene_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math import Color -from sdformat import Scene, Sky +from gz_test_deps.math import Color +from gz_test_deps.sdformat import Scene, Sky import unittest diff --git a/python/test/pySemanticPose_TEST.py b/python/test/pySemanticPose_TEST.py index 2a30d1993..6b514f605 100644 --- a/python/test/pySemanticPose_TEST.py +++ b/python/test/pySemanticPose_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math import Pose3d -from sdformat import Link, SemanticPose +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import Link, SemanticPose import unittest class semantic_poseTEST(unittest.TestCase): diff --git a/python/test/pySensor_TEST.py b/python/test/pySensor_TEST.py index d389e0efe..4a7f28415 100644 --- a/python/test/pySensor_TEST.py +++ b/python/test/pySensor_TEST.py @@ -13,11 +13,12 @@ # limitations under the License. import copy -from gz.math import Pose3d -from sdformat import (AirPressure, Altimeter, Camera, IMU, ForceTorque, Lidar, - Magnetometer, NavSat, Noise, Plugin, SemanticPose, - Sensor, SDFErrorsException) -import sdformat as sdf +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import (AirPressure, Altimeter, Camera, IMU, + ForceTorque, Lidar, Magnetometer, NavSat, + Noise, Plugin, SemanticPose, Sensor, + SDFErrorsException) +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pySky_TEST.py b/python/test/pySky_TEST.py index 7f58561dc..922da298d 100644 --- a/python/test/pySky_TEST.py +++ b/python/test/pySky_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math import Angle, Color -from sdformat import Sky +from gz_test_deps.math import Angle, Color +from gz_test_deps.sdformat import Sky import unittest diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index 370d574d5..0636d602b 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -13,9 +13,8 @@ # limitations under the License. import copy -from gz.math import Sphered import math -from sdformat import Sphere +from gz_test_deps.sdformat import Sphere import unittest class SphereTEST(unittest.TestCase): diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py index 27936dea4..2651b1113 100644 --- a/python/test/pySurface_TEST.py +++ b/python/test/pySurface_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math import Vector3d -from sdformat import Surface, Contact, Friction, ODE +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import Surface, Contact, Friction, ODE import unittest diff --git a/python/test/pyVisual_TEST.py b/python/test/pyVisual_TEST.py index 1733da573..bedadb0eb 100644 --- a/python/test/pyVisual_TEST.py +++ b/python/test/pyVisual_TEST.py @@ -13,9 +13,10 @@ # limitations under the License. import copy -from gz.math import Pose3d, Color -from sdformat import Geometry, Material, Visual, Plugin, SDFErrorsException -import sdformat as sdf +from gz_test_deps.math import Pose3d, Color +from gz_test_deps.sdformat import (Geometry, Material, Visual, Plugin, + SDFErrorsException) +import gz_test_deps.sdformat as sdf import unittest import math diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 88d929ccd..4048a349d 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -13,9 +13,10 @@ # limitations under the License. import copy -from gz.math import Color, Pose3d, Vector3d, SphericalCoordinates -from sdformat import Atmosphere, Gui, Physics, Plugin, Error, Frame, Light, Model, Scene, World -import sdformat as sdf +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) +import gz_test_deps.sdformat as sdf import unittest import math diff --git a/sdf/1.10/camera.sdf b/sdf/1.10/camera.sdf index 092aba97c..708e1d6eb 100644 --- a/sdf/1.10/camera.sdf +++ b/sdf/1.10/camera.sdf @@ -190,11 +190,37 @@ XY axis skew + + + Camera projection matrix P for overriding camera intrinsic matrix K values so that users can configure P independently of K. This is useful when working with stereo cameras where P may be different from K due to the transform between the two cameras. + + X focal length for projection matrix(in pixels, overrides fx) + + + Y focal length for projection matrix(in pixels, overrides fy) + + + X principal point for projection matrix(in pixels, overrides cx) + + + Y principal point for projection matrix(in pixels, overrides cy) + + + X translation for projection matrix (in pixels) + + + Y translation for projection matrix (in pixels) + + + + An optional frame id name to be used in the camera_info message header. + + diff --git a/sdf/1.10/collision_engine.sdf b/sdf/1.10/collision_engine.sdf deleted file mode 100644 index 67dbc7034..000000000 --- a/sdf/1.10/collision_engine.sdf +++ /dev/null @@ -1,17 +0,0 @@ - - - The collision_engine tag specifies the type and properties of the collision detection engine. - - - - The type of the collision detection engine. Current default in ODE is OPCODE. - - - - - - The type of the collision detection engine. - - - - diff --git a/sdf/1.10/joint.sdf b/sdf/1.10/joint.sdf index d472feefa..04d5f52e1 100644 --- a/sdf/1.10/joint.sdf +++ b/sdf/1.10/joint.sdf @@ -74,16 +74,16 @@ specifies the limits of this joint - + Specifies the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + Specifies the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + A value for enforcing the maximum joint effort applied. Limit is not enforced if value is negative. - + A value for enforcing the maximum joint velocity. @@ -132,16 +132,16 @@ - + An attribute specifying the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + An attribute specifying the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + An attribute for enforcing the maximum joint effort applied by Joint::SetForce. Limit is not enforced if value is negative. - + (not implemented) An attribute for enforcing the maximum joint velocity. diff --git a/sdf/1.10/urdf.sdf b/sdf/1.10/urdf.sdf deleted file mode 100644 index 6068d5cb6..000000000 --- a/sdf/1.10/urdf.sdf +++ /dev/null @@ -1,19 +0,0 @@ - - - The robot element defines a complete robot or any other physical object using URDF. - - - A unique name for the model. This name must not match another model in the world. - - - - A position and orientation in the global coordinate frame for the model. Position(x,y,z) and rotation (roll, pitch yaw) in the global coordinate frame. - - - - - - - - - diff --git a/sdf/1.7/camera.sdf b/sdf/1.7/camera.sdf index ef637048d..ec0bcafaa 100644 --- a/sdf/1.7/camera.sdf +++ b/sdf/1.7/camera.sdf @@ -155,11 +155,37 @@ XY axis skew + + + Camera projection matrix P for overriding camera intrinsic matrix K values so that users can configure P independently of K. This is useful when working with stereo cameras where P may be different from K due to the transform between the two cameras. + + X focal length for projection matrix(in pixels, overrides fx) + + + Y focal length for projection matrix(in pixels, overrides fy) + + + X principal point for projection matrix(in pixels, overrides cx) + + + Y principal point for projection matrix(in pixels, overrides cy) + + + X translation for projection matrix (in pixels) + + + Y translation for projection matrix (in pixels) + + + + An optional frame id name to be used in the camera_info message header. + + diff --git a/sdf/1.8/camera.sdf b/sdf/1.8/camera.sdf index ef637048d..ec0bcafaa 100644 --- a/sdf/1.8/camera.sdf +++ b/sdf/1.8/camera.sdf @@ -155,11 +155,37 @@ XY axis skew + + + Camera projection matrix P for overriding camera intrinsic matrix K values so that users can configure P independently of K. This is useful when working with stereo cameras where P may be different from K due to the transform between the two cameras. + + X focal length for projection matrix(in pixels, overrides fx) + + + Y focal length for projection matrix(in pixels, overrides fy) + + + X principal point for projection matrix(in pixels, overrides cx) + + + Y principal point for projection matrix(in pixels, overrides cy) + + + X translation for projection matrix (in pixels) + + + Y translation for projection matrix (in pixels) + + + + An optional frame id name to be used in the camera_info message header. + + diff --git a/sdf/1.9/camera.sdf b/sdf/1.9/camera.sdf index 092aba97c..708e1d6eb 100644 --- a/sdf/1.9/camera.sdf +++ b/sdf/1.9/camera.sdf @@ -190,11 +190,37 @@ XY axis skew + + + Camera projection matrix P for overriding camera intrinsic matrix K values so that users can configure P independently of K. This is useful when working with stereo cameras where P may be different from K due to the transform between the two cameras. + + X focal length for projection matrix(in pixels, overrides fx) + + + Y focal length for projection matrix(in pixels, overrides fy) + + + X principal point for projection matrix(in pixels, overrides cx) + + + Y principal point for projection matrix(in pixels, overrides cy) + + + X translation for projection matrix (in pixels) + + + Y translation for projection matrix (in pixels) + + + + An optional frame id name to be used in the camera_info message header. + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3465a8f2a..66affa10a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -121,9 +121,6 @@ if (WIN32) endif() target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} - PUBLIC - $ - $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ) diff --git a/src/Camera.cc b/src/Camera.cc index 607f9629a..1e63bcece 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -142,6 +142,9 @@ class sdf::Camera::Implementation /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; + /// \brief Frame ID the camera_info message header is expressed. + public: std::string opticalFrameId{""}; + /// \brief Lens type. public: std::string lensType{"stereographic"}; @@ -181,6 +184,24 @@ class sdf::Camera::Implementation /// \brief lens instrinsics cy. public: double lensIntrinsicsCy{120.0}; + /// \brief lens projection fx + public: double lensProjectionFx{277.0}; + + /// \brief lens projection fy + public: double lensProjectionFy{277.0}; + + /// \brief lens projection cx + public: double lensProjectionCx{160.0}; + + /// \brief lens projection cy + public: double lensProjectionCy{120.0}; + + /// \brief lens projection tx + public: double lensProjectionTx{0.0}; + + /// \brief lens projection ty + public: double lensProjectionTy{0.0}; + /// \brief lens instrinsics s. public: double lensIntrinsicsS{1.0}; @@ -348,6 +369,13 @@ Errors Camera::Load(ElementPtr _sdf) // Load the pose. Ignore the return value since the pose is optional. loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); + // Load the optional optical_frame_id value. + if (_sdf->HasElement("optical_frame_id")) + { + this->dataPtr->opticalFrameId = _sdf->Get("optical_frame_id", + this->dataPtr->opticalFrameId).first; + } + // Load the lens values. if (_sdf->HasElement("lens")) { @@ -392,6 +420,23 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->lensIntrinsicsS).first; this->dataPtr->hasIntrinsics = true; } + + if (elem->HasElement("projection")) { + sdf::ElementPtr projection = elem->GetElement("projection"); + this->dataPtr->lensProjectionFx = projection->Get("p_fx", + this->dataPtr->lensProjectionFx).first; + this->dataPtr->lensProjectionFy = projection->Get("p_fy", + this->dataPtr->lensProjectionFy).first; + this->dataPtr->lensProjectionCx = projection->Get("p_cx", + this->dataPtr->lensProjectionCx).first; + this->dataPtr->lensProjectionCy = projection->Get("p_cy", + this->dataPtr->lensProjectionCy).first; + this->dataPtr->lensProjectionTx = projection->Get("tx", + this->dataPtr->lensProjectionTx).first; + this->dataPtr->lensProjectionTy = projection->Get("ty", + this->dataPtr->lensProjectionTy).first; + + } } if (_sdf->HasElement("visibility_mask")) @@ -692,7 +737,8 @@ bool Camera::operator==(const Camera &_cam) const this->SaveFrames() == _cam.SaveFrames() && this->SaveFramesPath() == _cam.SaveFramesPath() && this->ImageNoise() == _cam.ImageNoise() && - this->VisibilityMask() == _cam.VisibilityMask(); + this->VisibilityMask() == _cam.VisibilityMask() && + this->OpticalFrameId() == _cam.OpticalFrameId(); } ////////////////////////////////////////////////// @@ -809,6 +855,18 @@ void Camera::SetPoseRelativeTo(const std::string &_frame) this->dataPtr->poseRelativeTo = _frame; } +///////////////////////////////////////////////// +const std::string Camera::OpticalFrameId() const +{ + return this->dataPtr->opticalFrameId; +} + +///////////////////////////////////////////////// +void Camera::SetOpticalFrameId(const std::string &_frame) +{ + this->dataPtr->opticalFrameId = _frame; +} + ///////////////////////////////////////////////// std::string Camera::LensType() const { @@ -969,6 +1027,78 @@ void Camera::SetLensIntrinsicsCy(double _cy) this->dataPtr->hasIntrinsics = true; } +///////////////////////////////////////////////// +double Camera::LensProjectionFx() const +{ + return this->dataPtr->lensProjectionFx; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionFx(double _fx_p) +{ + this->dataPtr->lensProjectionFx = _fx_p; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionFy() const +{ + return this->dataPtr->lensProjectionFy; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionFy(double _fy_p) +{ + this->dataPtr->lensProjectionFy = _fy_p; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionCx() const +{ + return this->dataPtr->lensProjectionCx; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionCx(double _cx_p) +{ + this->dataPtr->lensProjectionCx = _cx_p; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionCy() const +{ + return this->dataPtr->lensProjectionCy; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionCy(double _cy_p) +{ + this->dataPtr->lensProjectionCy = _cy_p; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionTx() const +{ + return this->dataPtr->lensProjectionTx; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionTx(double _tx) +{ + this->dataPtr->lensProjectionTx = _tx; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionTy() const +{ + return this->dataPtr->lensProjectionTy; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionTy(double _ty) +{ + this->dataPtr->lensProjectionTy = _ty; +} + ///////////////////////////////////////////////// double Camera::LensIntrinsicsSkew() const { @@ -1149,5 +1279,8 @@ sdf::ElementPtr Camera::ToElement() const this->SegmentationType()); } + elem->GetElement("optical_frame_id")->Set( + this->OpticalFrameId()); + return elem; } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 141b2e697..c10b6b05d 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -130,6 +130,10 @@ TEST(DOMCamera, Construction) cam.SetPoseRelativeTo("/frame"); EXPECT_EQ("/frame", cam.PoseRelativeTo()); + EXPECT_TRUE(cam.OpticalFrameId().empty()); + cam.SetOpticalFrameId("/optical_frame"); + EXPECT_EQ("/optical_frame", cam.OpticalFrameId()); + EXPECT_EQ("stereographic", cam.LensType()); cam.SetLensType("custom"); EXPECT_EQ("custom", cam.LensType()); @@ -182,6 +186,30 @@ TEST(DOMCamera, Construction) cam.SetLensIntrinsicsCy(123); EXPECT_DOUBLE_EQ(123, cam.LensIntrinsicsCy()); + EXPECT_DOUBLE_EQ(277, cam.LensProjectionFx()); + cam.SetLensProjectionFx(132); + EXPECT_DOUBLE_EQ(132, cam.LensProjectionFx()); + + EXPECT_DOUBLE_EQ(277, cam.LensProjectionFy()); + cam.SetLensProjectionFy(456); + EXPECT_DOUBLE_EQ(456, cam.LensProjectionFy()); + + EXPECT_DOUBLE_EQ(160, cam.LensProjectionCx()); + cam.SetLensProjectionCx(254); + EXPECT_DOUBLE_EQ(254, cam.LensProjectionCx()); + + EXPECT_DOUBLE_EQ(120, cam.LensProjectionCy()); + cam.SetLensProjectionCy(123); + EXPECT_DOUBLE_EQ(123, cam.LensProjectionCy()); + + EXPECT_DOUBLE_EQ(0, cam.LensProjectionTx()); + cam.SetLensProjectionTx(1); + EXPECT_DOUBLE_EQ(1, cam.LensProjectionTx()); + + EXPECT_DOUBLE_EQ(0, cam.LensProjectionTy()); + cam.SetLensProjectionTy(2); + EXPECT_DOUBLE_EQ(2, cam.LensProjectionTy()); + EXPECT_DOUBLE_EQ(1.0, cam.LensIntrinsicsSkew()); cam.SetLensIntrinsicsSkew(2.3); EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew()); @@ -240,6 +268,7 @@ TEST(DOMCamera, ToElement) cam.SetPoseRelativeTo("/frame"); cam.SetSaveFrames(true); cam.SetSaveFramesPath("/tmp"); + cam.SetOpticalFrameId("/optical_frame"); sdf::ElementPtr camElem = cam.ToElement(); EXPECT_NE(nullptr, camElem); @@ -259,6 +288,7 @@ TEST(DOMCamera, ToElement) EXPECT_EQ("/frame", cam2.PoseRelativeTo()); EXPECT_TRUE(cam2.SaveFrames()); EXPECT_EQ("/tmp", cam2.SaveFramesPath()); + EXPECT_EQ("/optical_frame", cam2.OpticalFrameId()); // make changes to DOM and verify ToElement produces updated values cam2.SetNearClip(0.33); diff --git a/src/Collision.cc b/src/Collision.cc index b0b33aa24..d45b47877 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -64,6 +64,12 @@ Collision::Collision() ///////////////////////////////////////////////// Errors Collision::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -98,7 +104,8 @@ Errors Collision::Load(ElementPtr _sdf) loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); // Load the geometry - Errors geomErr = this->dataPtr->geom.Load(_sdf->GetElement("geometry")); + Errors geomErr = this->dataPtr->geom.Load( + _sdf->GetElement("geometry"), _config); errors.insert(errors.end(), geomErr.begin(), geomErr.end()); // Load the surface parameters if they are given diff --git a/src/Geometry.cc b/src/Geometry.cc index ceae9cd72..e31a5e69b 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -78,6 +78,12 @@ Geometry::Geometry() ///////////////////////////////////////////////// Errors Geometry::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Geometry::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -148,14 +154,15 @@ Errors Geometry::Load(ElementPtr _sdf) { this->dataPtr->type = GeometryType::MESH; this->dataPtr->mesh.emplace(); - Errors err = this->dataPtr->mesh->Load(_sdf->GetElement("mesh")); + Errors err = this->dataPtr->mesh->Load(_sdf->GetElement("mesh"), _config); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("heightmap")) { this->dataPtr->type = GeometryType::HEIGHTMAP; this->dataPtr->heightmap.emplace(); - Errors err = this->dataPtr->heightmap->Load(_sdf->GetElement("heightmap")); + Errors err = this->dataPtr->heightmap->Load( + _sdf->GetElement("heightmap"), _config); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("polyline")) diff --git a/src/Heightmap.cc b/src/Heightmap.cc index 2a57a67cf..b9a5619e5 100644 --- a/src/Heightmap.cc +++ b/src/Heightmap.cc @@ -91,6 +91,12 @@ HeightmapTexture::HeightmapTexture() ///////////////////////////////////////////////// Errors HeightmapTexture::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors HeightmapTexture::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -126,8 +132,9 @@ Errors HeightmapTexture::Load(ElementPtr _sdf) if (_sdf->HasElement("diffuse")) { - this->dataPtr->diffuse = _sdf->Get("diffuse", - this->dataPtr->diffuse).first; + this->dataPtr->diffuse = resolveURI( + _sdf->Get("diffuse", this->dataPtr->diffuse).first, + _config, errors); } else { @@ -137,8 +144,9 @@ Errors HeightmapTexture::Load(ElementPtr _sdf) if (_sdf->HasElement("normal")) { - this->dataPtr->normal = _sdf->Get("normal", - this->dataPtr->normal).first; + this->dataPtr->normal = resolveURI( + _sdf->Get("normal", this->dataPtr->normal).first, + _config, errors); } else { @@ -285,6 +293,12 @@ Heightmap::Heightmap() ///////////////////////////////////////////////// Errors Heightmap::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Heightmap::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -311,7 +325,9 @@ Errors Heightmap::Load(ElementPtr _sdf) if (_sdf->HasElement("uri")) { - this->dataPtr->uri = _sdf->Get("uri", "").first; + this->dataPtr->uri = resolveURI( + _sdf->Get("uri", "").first, + _config, errors); } else { @@ -332,7 +348,7 @@ Errors Heightmap::Load(ElementPtr _sdf) this->dataPtr->sampling).first; Errors textureLoadErrors = loadRepeated(_sdf, - "texture", this->dataPtr->textures); + "texture", this->dataPtr->textures, _config); errors.insert(errors.end(), textureLoadErrors.begin(), textureLoadErrors.end()); diff --git a/src/JointAxis.cc b/src/JointAxis.cc index de3ef479d..735254ea1 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include #include @@ -41,10 +42,6 @@ class sdf::JointAxis::Implementation /// \brief Frame in which xyz is expressed in. public: std::string xyzExpressedIn = ""; - /// \brief Flag to interpret the axis xyz element in the parent model - /// frame instead of joint frame. - public: bool useParentModelFrame = false; - /// \brief The physical velocity dependent viscous damping coefficient /// of the joint. public: double damping = 0.0; @@ -60,18 +57,18 @@ class sdf::JointAxis::Implementation /// \brief Specifies the lower joint limit (radians for revolute joints, /// meters for prismatic joints). Omit if joint is continuous. - public: double lower = -1e16; + public: double lower = -std::numeric_limits::infinity(); /// \brief Specifies the upper joint limit (radians for revolute joints, /// meters for prismatic joints). Omit if joint is continuous. - public: double upper = 1e16; + public: double upper = std::numeric_limits::infinity(); /// \brief A value for enforcing the maximum joint effort applied. /// Limit is not enforced if value is negative. - public: double effort = -1; + public: double effort = std::numeric_limits::infinity(); /// \brief A value for enforcing the maximum joint velocity. - public: double maxVelocity = -1; + public: double maxVelocity = std::numeric_limits::infinity(); /// \brief Joint stop stiffness. public: double stiffness = 1e8; @@ -106,7 +103,8 @@ Errors JointAxis::Load(ElementPtr _sdf) if (_sdf->HasElement("xyz")) { using gz::math::Vector3d; - auto errs = this->SetXyz(_sdf->Get("xyz", Vector3d::UnitZ).first); + auto errs = this->SetXyz(_sdf->Get("xyz", + this->dataPtr->xyz).first); std::copy(errs.begin(), errs.end(), std::back_inserter(errors)); auto e = _sdf->GetElement("xyz"); if (e->HasAttribute("expressed_in")) @@ -120,21 +118,19 @@ Errors JointAxis::Load(ElementPtr _sdf) "The xyz element in joint axis is required"}); } - // Get whether to use the parent model frame. - this->dataPtr->useParentModelFrame = _sdf->Get( - "use_parent_model_frame", false).first; - // Load dynamic values, if present if (_sdf->HasElement("dynamics")) { sdf::ElementPtr dynElement = _sdf->GetElement("dynamics"); - this->dataPtr->damping = dynElement->Get("damping", 0.0).first; - this->dataPtr->friction = dynElement->Get("friction", 0.0).first; - this->dataPtr->springReference = - dynElement->Get("spring_reference", 0.0).first; - this->dataPtr->springStiffness = - dynElement->Get("spring_stiffness", 0.0).first; + this->dataPtr->damping = dynElement->Get("damping", + this->dataPtr->damping).first; + this->dataPtr->friction = dynElement->Get("friction", + this->dataPtr->friction).first; + this->dataPtr->springReference = dynElement->Get("spring_reference", + this->dataPtr->springReference).first; + this->dataPtr->springStiffness = dynElement->Get("spring_stiffness", + this->dataPtr->springStiffness).first; } // Load limit values @@ -142,15 +138,18 @@ Errors JointAxis::Load(ElementPtr _sdf) { sdf::ElementPtr limitElement = _sdf->GetElement("limit"); - this->dataPtr->lower = limitElement->Get("lower", -1e16).first; - this->dataPtr->upper = limitElement->Get("upper", 1e16).first; - this->dataPtr->effort = limitElement->Get("effort", -1).first; - this->dataPtr->maxVelocity = limitElement->Get( - "velocity", -1).first; - this->dataPtr->stiffness = limitElement->Get( - "stiffness", 1e8).first; - this->dataPtr->dissipation = limitElement->Get( - "dissipation", 1.0).first; + this->dataPtr->lower = limitElement->Get("lower", + this->dataPtr->lower).first; + this->dataPtr->upper = limitElement->Get("upper", + this->dataPtr->upper).first; + this->dataPtr->effort = limitElement->Get("effort", + this->dataPtr->effort).first; + this->dataPtr->maxVelocity = limitElement->Get("velocity", + this->dataPtr->maxVelocity).first; + this->dataPtr->stiffness = limitElement->Get("stiffness", + this->dataPtr->stiffness).first; + this->dataPtr->dissipation = limitElement->Get("dissipation", + this->dataPtr->dissipation).first; } else { @@ -392,11 +391,6 @@ sdf::ElementPtr JointAxis::ToElement(unsigned int _index) const xyzElem->GetAttribute("expressed_in")->Set( this->XyzExpressedIn()); } - else if (this->dataPtr->useParentModelFrame) - { - xyzElem->GetAttribute("expressed_in")->Set( - "__model__"); - } sdf::ElementPtr dynElem = axisElem->GetElement("dynamics"); dynElem->GetElement("damping")->Set(this->Damping()); dynElem->GetElement("friction")->Set(this->Friction()); diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index 546dae334..031c989a6 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include "sdf/JointAxis.hh" ///////////////////////////////////////////////// @@ -29,8 +30,8 @@ TEST(DOMJointAxis, Construction) EXPECT_DOUBLE_EQ(0.0, axis.Friction()); EXPECT_DOUBLE_EQ(0.0, axis.SpringReference()); EXPECT_DOUBLE_EQ(0.0, axis.SpringStiffness()); - EXPECT_DOUBLE_EQ(-1e16, axis.Lower()); - EXPECT_DOUBLE_EQ(1e16, axis.Upper()); + EXPECT_DOUBLE_EQ(-std::numeric_limits::infinity(), axis.Lower()); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.Upper()); EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.Effort()); EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.MaxVelocity()); EXPECT_DOUBLE_EQ(1e8, axis.Stiffness()); diff --git a/src/Link.cc b/src/Link.cc index 3ac2413e2..859848c4e 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -86,6 +86,12 @@ Link::Link() ///////////////////////////////////////////////// Errors Link::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -121,12 +127,12 @@ Errors Link::Load(ElementPtr _sdf) // Load all the visuals. Errors visLoadErrors = loadUniqueRepeated(_sdf, "visual", - this->dataPtr->visuals); + this->dataPtr->visuals, _config); errors.insert(errors.end(), visLoadErrors.begin(), visLoadErrors.end()); // Load all the collisions. Errors collLoadErrors = loadUniqueRepeated(_sdf, "collision", - this->dataPtr->collisions); + this->dataPtr->collisions, _config); errors.insert(errors.end(), collLoadErrors.begin(), collLoadErrors.end()); // Load all the lights. diff --git a/src/Material.cc b/src/Material.cc index 9f1cbfcc0..c5c41c394 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -83,6 +83,12 @@ Material::Material() ///////////////////////////////////////////////// Errors Material::Load(sdf::ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Material::Load(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config) { Errors errors; @@ -114,7 +120,8 @@ Errors Material::Load(sdf::ElementPtr _sdf) "A + + + + + + + shapes.sdf + + + + diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index af69d9ba9..e7dc5f959 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -75,6 +75,14 @@ 124 1.2 + + 280 + 281 + 162 + 124 + 0 + 0 +