From 8e558ee70ee0f44353a5f5ca3970a7920e7c1ae7 Mon Sep 17 00:00:00 2001 From: chapulina Date: Tue, 21 Apr 2020 19:18:26 -0700 Subject: [PATCH 01/60] [sdf6] Changelog links to BitBucket backup (#237) * [sdf6] Changelog links to BitBucket backup Signed-off-by: Louise Poubel * more fixes, new version of script Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 358 ++++++++++++++++++++-------------------- INSTALL_WIN32.md | 2 +- Migration.md | 44 ++--- bitbucket-pipelines.yml | 2 +- doc/header.html | 2 +- doc/mainpage.html | 2 +- 7 files changed, 206 insertions(+), 206 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 78e72e624..a25b421fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,7 @@ set (CMAKE_INCLUDE_DIRECTORIES_PROJECT_BEFORE ON) # to choose the flag -std=gnu++14 instead of -std=c++14 when the C++14 # features are requested. Explicitly turning this flag off will force cmake to # choose -std=c++14. -# See https://bitbucket.org/ignitionrobotics/ign-cmake/issues/13 for more info. +# See https://github.com/ignitionrobotics/ign-cmake/issues/13 for more info. set(CMAKE_CXX_EXTENSIONS off) # Include GNUInstallDirs to get canonical paths diff --git a/Changelog.md b/Changelog.md index bcacf2222..7763b968f 100644 --- a/Changelog.md +++ b/Changelog.md @@ -3,85 +3,85 @@ ### SDFormat 6.X.X (20XX-XX-XX) 1. Parse urdf files to sdf 1.5 instead of 1.4 to avoid `use_parent_model_frame`. - * [Pull request 575](https://bitbucket.org/osrf/sdformat/pull-requests/575) + * [BitBucket pull request 575](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/575) 1. Set camera intrinsics axis skew (s) default value to 0 - * [Pull request 504](https://bitbucket.org/osrf/sdformat/pull-requests/504) + * [BitBucket pull request 504](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/504) 1. Avoid hardcoding /machine:x64 flag on 64-bit on MSVC with CMake >= 3.5. - * [Pull request 565](https://bitbucket.org/osrf/sdformat/pull-requests/565) + * [BitBucket pull request 565](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/565) 1. Fix ign library path on macOS. - * [Pull request 552](https://bitbucket.org/osrf/sdformat/pull-requests/552) + * [BitBucket pull request 552](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/552) 1. Use `ign sdf --check` to check sibling elements of the same type for non-unique names. - * [Pull request 554](https://bitbucket.org/osrf/sdformat/pull-requests/554) + * [BitBucket pull request 554](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/554) 1. Converter: remove all matching elements specified by `` tag. - * [Pull request 551](https://bitbucket.org/osrf/sdformat/pull-requests/551) + * [BitBucket pull request 551](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/551) ### SDFormat 6.2.0 (2019-01-17) 1. Add geometry for sonar collision shape - * [Pull request 495](https://bitbucket.org/osrf/sdformat/pull-requests/495) + * [BitBucket pull request 495](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/495) 1. Add camera intrinsics (fx, fy, cx, cy, s) - * [Pull request 496](https://bitbucket.org/osrf/sdformat/pull-requests/496) + * [BitBucket pull request 496](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/496) 1. Add actor trajectory tension parameter - * [Pull request 466](https://bitbucket.org/osrf/sdformat/pull-requests/466) + * [BitBucket pull request 466](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/466) ### SDFormat 6.1.0 (2018-10-04) 1. Add collision\_detector to dart physics config - * [Pull request 440](https://bitbucket.org/osrf/sdformat/pull-requests/440) + * [BitBucket pull request 440](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/440) 1. Fix Windows support for SDFormat6 - * [Pull request 401](https://bitbucket.org/osrf/sdformat/pull-requests/401) + * [BitBucket pull request 401](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/401) 1. root.sdf: default sdf version 1.6 - * [Pull request 425](https://bitbucket.org/osrf/sdformat/pull-requests/425) + * [BitBucket pull request 425](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/425) 1. parser\_urdf: print value of highstop instead of pointer address - * [Pull request 408](https://bitbucket.org/osrf/sdformat/pull-requests/408) + * [BitBucket pull request 408](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/408) 1. Tweak error output so jenkins doesn't think it's a compiler warning - * [Pull request 402](https://bitbucket.org/osrf/sdformat/pull-requests/402) + * [BitBucket pull request 402](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/402) ### SDFormat 6.0.0 (2018-01-25) 1. SDF DOM: Added a document object model. - * [Pull request 387](https://bitbucket.org/osrf/sdformat/pull-requests/387) - * [Pull request 389](https://bitbucket.org/osrf/sdformat/pull-requests/389) + * [BitBucket pull request 387](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/387) + * [BitBucket pull request 389](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/389) 1. Add simplified ``readFile`` function. - * [Pull request 347](https://bitbucket.org/osrf/sdformat/pull-requests/347) + * [BitBucket pull request 347](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/347) 1. Remove boost::lexical cast instances - * [Pull request 342](https://bitbucket.org/osrf/sdformat/pull-requests/342) + * [BitBucket pull request 342](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/342) 1. Remove boost regex and iostreams as dependencies - * [Pull request 302](https://bitbucket.org/osrf/sdformat/pull-requests/302) + * [BitBucket pull request 302](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/302) 1. Change certain error checks from asserts to throwing sdf::AssertionInternalError, which is more appropriate for a library. - * [Pull request 315](https://bitbucket.org/osrf/sdformat/pull-requests/315) + * [BitBucket pull request 315](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/315) 1. Updated the internal copy of urdfdom to 1.0, removing more of boost. - * [Pull request 324](https://bitbucket.org/osrf/sdformat/pull-requests/324) + * [BitBucket pull request 324](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/324) 1. urdfdom 1.0 is now required on all platforms. - * [Pull request 324](https://bitbucket.org/osrf/sdformat/pull-requests/324) + * [BitBucket pull request 324](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/324) 1. Remove boost filesystem as a dependency - * [Pull request 335](https://bitbucket.org/osrf/sdformat/pull-requests/335) - * [Pull request 338](https://bitbucket.org/osrf/sdformat/pull-requests/338) - * [Pull request 339](https://bitbucket.org/osrf/sdformat/pull-requests/339) + * [BitBucket pull request 335](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/335) + * [BitBucket pull request 338](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/338) + * [BitBucket pull request 339](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/339) 1. Deprecated sdf::Color, and switch to use ignition::math::Color - * [Pull request 330](https://bitbucket.org/osrf/sdformat/pull-requests/330) + * [BitBucket pull request 330](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/330) ## SDFormat 5.x @@ -90,83 +90,83 @@ ### SDFormat 5.3.0 (2017-11-13) 1. Added wrapper around root SDF for an SDF element - * [Pull request 378](https://bitbucket.org/osrf/sdformat/pull-request/378) - * [Pull request 372](https://bitbucket.org/osrf/sdformat/pull-request/372) + * [BitBucket pull request 378](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/378) + * [BitBucket pull request 372](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/372) 1. Add ODE parallelization parameters: threaded islands and position correction - * [Pull request 380](https://bitbucket.org/osrf/sdformat/pull-request/380) + * [BitBucket pull request 380](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/380) 1. surface.sdf: expand documentation of friction and slip coefficients - * [Pull request 343](https://bitbucket.org/osrf/sdformat/pull-request/343) + * [BitBucket pull request 343](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/343) 1. Add preserveFixedJoint option to the URDF parser - * [Pull request 352](https://bitbucket.org/osrf/sdformat/pull-request/352) + * [BitBucket pull request 352](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/352) 1. Add light as child of link - * [Pull request 373](https://bitbucket.org/osrf/sdformat/pull-request/373) + * [BitBucket pull request 373](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/373) ### SDFormat 5.2.0 (2017-08-03) 1. Added a block for DART-specific physics properties. - * [Pull request 369](https://bitbucket.org/osrf/sdformat/pull-requests/369) + * [BitBucket pull request 369](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/369) 1. Fix parser to read plugin child elements within an `` - * [Pull request 350](https://bitbucket.org/osrf/sdformat/pull-request/350) + * [BitBucket pull request 350](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/350) 1. Choosing models with more recent sdf version with `` tag - * [Pull request 291](https://bitbucket.org/osrf/sdformat/pull-request/291) - * [Issue 123](https://bitbucket.org/osrf/sdformat/issues/123) + * [BitBucket pull request 291](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/291) + * [Issue 123](https://github.com/osrf/sdformat/issues/123) 1. Added `` to 1.6 surface contact parameters - * [Pull request 318](https://bitbucket.org/osrf/sdformat/pull-request/318) + * [BitBucket pull request 318](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/318) 1. Support light insertion in state - * [Pull request 325](https://bitbucket.org/osrf/sdformat/pull-request/325) + * [BitBucket pull request 325](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/325) 1. Case insensitive boolean strings - * [Pull request 322](https://bitbucket.org/osrf/sdformat/pull-request/322) + * [BitBucket pull request 322](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/322) 1. Enable coverage testing - * [Pull request 317](https://bitbucket.org/osrf/sdformat/pull-request/317) + * [BitBucket pull request 317](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/317) 1. Add `friction_model` parameter to ode solver - * [Pull request 294](https://bitbucket.org/osrf/sdformat/pull-request/294) - * [Gazebo pull request 1522](https://bitbucket.org/osrf/gazebo/pull-request/1522) + * [BitBucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/294) + * [Gazebo pull request 1522](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1522) 1. Add cmake `@PKG_NAME@_LIBRARY_DIRS` variable to cmake config file - * [Pull request 292](https://bitbucket.org/osrf/sdformat/pull-request/292) + * [BitBucket pull request 292](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/292) ### SDFormat 5.1.0 (2017-02-22) 1. Fixed `sdf::convertFile` and `sdf::convertString` always converting to latest version - * [Pull request 320](https://bitbucket.org/osrf/sdformat/pull-requests/320) + * [BitBucket pull request 320](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/320) 1. Added back the ability to set sdf version at runtime - * [Pull request 307](https://bitbucket.org/osrf/sdformat/pull-requests/307) + * [BitBucket pull request 307](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/307) ### SDFormat 5.0.0 (2017-01-25) 1. Removed SDFormat 4 deprecations - * [Pull request 295](https://bitbucket.org/osrf/sdformat/pull-requests/295) + * [BitBucket pull request 295](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/295) 1. Added an example - * [Pull request 275](https://bitbucket.org/osrf/sdformat/pull-requests/275) + * [BitBucket pull request 275](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/275) 1. Move functions that use TinyXML classes in private headers A contribution from Silvio Traversaro - * [Pull request 262](https://bitbucket.org/osrf/sdformat/pull-requests/262) + * [BitBucket pull request 262](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/262) 1. Fix issues found by the Coverity tool A contribution from Olivier Crave - * [Pull request 259](https://bitbucket.org/osrf/sdformat/pull-requests/259) + * [BitBucket pull request 259](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/259) 1. Add tag to allow for specification of initial joint position - * [Pull request 279](https://bitbucket.org/osrf/sdformat/pull-requests/279) + * [BitBucket pull request 279](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/279) 1. Require ignition-math3 as dependency - * [Pull request 299](https://bitbucket.org/osrf/sdformat/pull-requests/299) + * [BitBucket pull request 299](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/299) 1. Simplifier way of retrieving a value from SDF using Get - * [Pull request 285](https://bitbucket.org/osrf/sdformat/pull-requests/285) + * [BitBucket pull request 285](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/285) ## SDFormat 4.0 @@ -175,337 +175,337 @@ ### SDFormat 4.4.0 (2017-10-26) 1. Add ODE parallelization parameters: threaded islands and position correction - * [Pull request 380](https://bitbucket.org/osrf/sdformat/pull-request/380) + * [BitBucket pull request 380](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/380) 1. surface.sdf: expand documentation of friction and slip coefficients - * [Pull request 343](https://bitbucket.org/osrf/sdformat/pull-request/343) + * [BitBucket pull request 343](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/343) 1. Add preserveFixedJoint option to the URDF parser - * [Pull request 352](https://bitbucket.org/osrf/sdformat/pull-request/352) + * [BitBucket pull request 352](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/352) 1. Add light as child of link - * [Pull request 373](https://bitbucket.org/osrf/sdformat/pull-request/373) + * [BitBucket pull request 373](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/373) ### SDFormat 4.3.2 (2017-07-19) 1. Add documentation for `Element::GetFirstElement()` and `Element::GetNextElement()` - * [Pull request 341](https://bitbucket.org/osrf/sdformat/pull-request/341) + * [BitBucket pull request 341](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/341) 1. Fix parser to read plugin child elements within an `` - * [Pull request 350](https://bitbucket.org/osrf/sdformat/pull-request/350) + * [BitBucket pull request 350](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/350) ### SDFormat 4.3.1 (2017-03-24) 1. Fix segmentation Fault in `sdf::getBestSupportedModelVersion` - * [Pull request 327](https://bitbucket.org/osrf/sdformat/pull-requests/327) - * [Issue 152](https://bitbucket.org/osrf/sdformat/issues/152) + * [BitBucket pull request 327](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/327) + * [Issue 152](https://github.com/osrf/sdformat/issues/152) ### SDFormat 4.3.0 (2017-03-20) 1. Choosing models with more recent sdf version with `` tag - * [Pull request 291](https://bitbucket.org/osrf/sdformat/pull-request/291) - * [Issue 123](https://bitbucket.org/osrf/sdformat/issues/123) + * [BitBucket pull request 291](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/291) + * [Issue 123](https://github.com/osrf/sdformat/issues/123) 1. Added `` to 1.6 surface contact parameters - * [Pull request 318](https://bitbucket.org/osrf/sdformat/pull-request/318) + * [BitBucket pull request 318](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/318) 1. Support light insertion in state - * [Pull request 325](https://bitbucket.org/osrf/sdformat/pull-request/325) + * [BitBucket pull request 325](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/325) 1. Case insensitive boolean strings - * [Pull request 322](https://bitbucket.org/osrf/sdformat/pull-request/322) + * [BitBucket pull request 322](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/322) 1. Enable coverage testing - * [Pull request 317](https://bitbucket.org/osrf/sdformat/pull-request/317) + * [BitBucket pull request 317](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/317) 1. Add `friction_model` parameter to ode solver - * [Pull request 294](https://bitbucket.org/osrf/sdformat/pull-request/294) - * [Gazebo pull request 1522](https://bitbucket.org/osrf/gazebo/pull-request/1522) + * [BitBucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/294) + * [Gazebo pull request 1522](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1522) 1. Added `sampling` parameter to `` SDF element. - * [Pull request 293](https://bitbucket.org/osrf/sdformat/pull-request/293) + * [BitBucket pull request 293](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/293) 1. Added Migration guide - * [Pull request 290](https://bitbucket.org/osrf/sdformat/pull-request/290) + * [BitBucket pull request 290](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/290) 1. Add cmake `@PKG_NAME@_LIBRARY_DIRS` variable to cmake config file - * [Pull request 292](https://bitbucket.org/osrf/sdformat/pull-request/292) + * [BitBucket pull request 292](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/292) ### SDFormat 4.2.0 (2016-10-10) 1. Added tag to specify ODE friction model. - * [Pull request 294](https://bitbucket.org/osrf/sdformat/pull-request/294) + * [BitBucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/294) 1. Fix URDF to SDF `self_collide` bug. - * [Pull request 287](https://bitbucket.org/osrf/sdformat/pull-request/287) + * [BitBucket pull request 287](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/287) 1. Added IMU orientation specification to SDF. - * [Pull request 284](https://bitbucket.org/osrf/sdformat/pull-request/284) + * [BitBucket pull request 284](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/284) ### SDFormat 4.1.1 (2016-07-08) 1. Added documentation and animation to `` element. - * [Pull request 280](https://bitbucket.org/osrf/sdformat/pull-request/280) + * [BitBucket pull request 280](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/280) 1. Added tag to specify initial joint position - * [Pull request 279](https://bitbucket.org/osrf/sdformat/pull-request/279) + * [BitBucket pull request 279](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/279) ### SDFormat 4.1.0 (2016-04-01) 1. Added SDF conversion functions to parser including sdf::convertFile and sdf::convertString. - * [Pull request 266](https://bitbucket.org/osrf/sdformat/pull-request/266) + * [BitBucket pull request 266](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/266) 1. Added an upload script - * [Pull request 256](https://bitbucket.org/osrf/sdformat/pull-request/256) + * [BitBucket pull request 256](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/256) ### SDFormat 4.0.0 (2015-01-12) 1. Boost pointers and boost::function in the public API have been replaced by their std::equivalents (C++11 standard) 1. Move gravity and magnetic_field tags from physics to world - * [Pull request 247](https://bitbucket.org/osrf/sdformat/pull-request/247) + * [BitBucket pull request 247](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/247) 1. Switch lump link prefix from lump:: to lump_ - * [Pull request 245](https://bitbucket.org/osrf/sdformat/pull-request/245) + * [BitBucket pull request 245](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/245) 1. New element. A contribution from Olivier Crave - * [Pull request 240](https://bitbucket.org/osrf/sdformat/pull-request/240) + * [BitBucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/240) 1. Add scale to model state - * [Pull request 246](https://bitbucket.org/osrf/sdformat/pull-request/246) + * [BitBucket pull request 246](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/246) 1. Use stof functions to parse hex strings as floating point params. A contribution from Rich Mattes - * [Pull request 250](https://bitbucket.org/osrf/sdformat/pull-request/250) + * [BitBucket pull request 250](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/250) 1. Fix memory leaks. A contribution from Silvio Traversaro - * [Pull request 249](https://bitbucket.org/osrf/sdformat/pull-request/249) + * [BitBucket pull request 249](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/249) 1. Update SDF to version 1.6: new style for representing the noise properties of an `imu` - * [Pull request 243](https://bitbucket.org/osrf/sdformat/pull-request/243) - * [Pull request 199](https://bitbucket.org/osrf/sdformat/pull-requests/199) + * [BitBucket pull request 243](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/243) + * [BitBucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) ## SDFormat 3.0 ### SDFormat 3.X.X (201X-XX-XX) 1. Improve precision of floating point parameters - * [Pull request 273](https://bitbucket.org/osrf/sdformat/pull-requests/273) - * [Pull request 276](https://bitbucket.org/osrf/sdformat/pull-requests/276) + * [BitBucket pull request 273](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/273) + * [BitBucket pull request 276](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/276) ### SDFormat 3.7.0 (2015-11-20) 1. Add spring pass through for sdf3 - * [Design document](https://bitbucket.org/osrf/gazebo_design/pull-requests/23) - * [Pull request 242](https://bitbucket.org/osrf/sdformat/pull-request/242) + * [Design document](https://bitbucket.org/osrf/gazebo_design/pull-requests/23) + * [BitBucket pull request 242](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/242) 1. Support frame specification in SDF - * [Pull request 237](https://bitbucket.org/osrf/sdformat/pull-request/237) + * [BitBucket pull request 237](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/237) 1. Remove boost from SDFExtension - * [Pull request 229](https://bitbucket.org/osrf/sdformat/pull-request/229) + * [BitBucket pull request 229](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/229) ### SDFormat 3.6.0 (2015-10-27) 1. Add light state - * [Pull request 227](https://bitbucket.org/osrf/sdformat/pull-request/227) + * [BitBucket pull request 227](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/227) 1. redo pull request #222 for sdf3 branch - * [Pull request 232](https://bitbucket.org/osrf/sdformat/pull-request/232) + * [BitBucket pull request 232](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/232) 1. Fix links in API documentation - * [Pull request 231](https://bitbucket.org/osrf/sdformat/pull-request/231) + * [BitBucket pull request 231](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/231) ### SDFormat 3.5.0 (2015-10-07) 1. Camera lens description (Replaces #213) - * [Pull request 215](https://bitbucket.org/osrf/sdformat/pull-request/215) + * [BitBucket pull request 215](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/215) 1. Fix shared pointer reference loop in Element and memory leak (#104) - * [Pull request 230](https://bitbucket.org/osrf/sdformat/pull-request/230) + * [BitBucket pull request 230](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/230) ### SDFormat 3.4.0 (2015-10-05) 1. Support nested model states - * [Pull request 223](https://bitbucket.org/osrf/sdformat/pull-request/223) + * [BitBucket pull request 223](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/223) 1. Cleaner way to set SDF_PATH for tests - * [Pull request 226](https://bitbucket.org/osrf/sdformat/pull-request/226) + * [BitBucket pull request 226](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/226) ### SDFormat 3.3.0 (2015-09-15) 1. Windows Boost linking errors - * [Pull request 206](https://bitbucket.org/osrf/sdformat/pull-request/206) + * [BitBucket pull request 206](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/206) 1. Nested SDF -> sdf3 - * [Pull request 221](https://bitbucket.org/osrf/sdformat/pull-request/221) + * [BitBucket pull request 221](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/221) 1. Pointer types - * [Pull request 218](https://bitbucket.org/osrf/sdformat/pull-request/218) + * [BitBucket pull request 218](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/218) 1. Torsional friction default surface radius not infinity - * [Pull request 217](https://bitbucket.org/osrf/sdformat/pull-request/217) + * [BitBucket pull request 217](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/217) ### SDFormat 3.2.2 (2015-08-24) 1. Added battery element (contribution from Olivier Crave) - * [Pull request #204](https://bitbucket.org/osrf/sdformat/pull-request/204) + * [BitBucket pull request #204](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/204) 1. Torsional friction backport - * [Pull request #211](https://bitbucket.org/osrf/sdformat/pull-request/211) + * [BitBucket pull request #211](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/211) 1. Allow Visual Studio 2015 - * [Pull request #208](https://bitbucket.org/osrf/sdformat/pull-request/208) + * [BitBucket pull request #208](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/208) ### SDFormat 3.1.1 (2015-08-03) 1. Fix tinyxml linking error - * [Pull request #209](https://bitbucket.org/osrf/sdformat/pull-request/209) + * [BitBucket pull request #209](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/209) ### SDFormat 3.1.0 (2015-08-02) 1. Added logical camera sensor to SDF - * [Pull request #207](https://bitbucket.org/osrf/sdformat/pull-request/207) + * [BitBucket pull request #207](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/207) ### SDFormat 3.0.0 (2015-07-24) 1. Added battery to SDF - * [Pull request 204](https://bitbucket.org/osrf/sdformat/pull-request/204) + * [BitBucket pull request 204](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/204) 1. Added altimeter sensor to SDF - * [Pull request #197](https://bitbucket.org/osrf/sdformat/pull-request/197) + * [BitBucket pull request #197](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/197) 1. Added magnetometer sensor to SDF - * [Pull request 198](https://bitbucket.org/osrf/sdformat/pull-request/198) + * [BitBucket pull request 198](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/198) 1. Fix detection of XML parsing errors - * [Pull request 190](https://bitbucket.org/osrf/sdformat/pull-request/190) + * [BitBucket pull request 190](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/190) 1. Support for fixed joints - * [Pull request 194](https://bitbucket.org/osrf/sdformat/pull-request/194) + * [BitBucket pull request 194](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/194) 1. Adding iterations to state - * [Pull request 188](https://bitbucket.org/osrf/sdformat/pull-request/188) + * [BitBucket pull request 188](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/188) 1. Convert to use ignition-math - * [Pull request 173](https://bitbucket.org/osrf/sdformat/pull-request/173) + * [BitBucket pull request 173](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/173) 1. Add world origin to scene - * [Pull request 183](https://bitbucket.org/osrf/sdformat/pull-request/183) + * [BitBucket pull request 183](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/183) 1. Fix collide bitmask - * [Pull request 182](https://bitbucket.org/osrf/sdformat/pull-request/182) + * [BitBucket pull request 182](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/182) 1. Adding meta information to visuals - * [Pull request 180](https://bitbucket.org/osrf/sdformat/pull-request/180) + * [BitBucket pull request 180](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/180) 1. Add projection type to gui camera - * [Pull request 178](https://bitbucket.org/osrf/sdformat/pull-request/178) + * [BitBucket pull request 178](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/178) 1. Fix print description to include attribute description - * [Pull request 170](https://bitbucket.org/osrf/sdformat/pull-request/170) + * [BitBucket pull request 170](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/170) 1. Add -std=c++11 flag to sdf_config.cmake.in and sdformat.pc.in, needed by downstream code - * [Pull request 172](https://bitbucket.org/osrf/sdformat/pull-request/172) + * [BitBucket pull request 172](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/172) 1. Added boost::any accessor for Param and Element - * [Pull request 166](https://bitbucket.org/osrf/sdformat/pull-request/166) + * [BitBucket pull request 166](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/166) 1. Remove tinyxml from dependency list - * [Pull request 152](https://bitbucket.org/osrf/sdformat/pull-request/152) + * [BitBucket pull request 152](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/152) 1. Added self_collide element for model - * [Pull request 149](https://bitbucket.org/osrf/sdformat/pull-request/149) + * [BitBucket pull request 149](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/149) 1. Added a collision bitmask field to sdf-1.5 and c++11 support - * [Pull request 145](https://bitbucket.org/osrf/sdformat/pull-request/145) + * [BitBucket pull request 145](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/145) 1. Fix problems with latin locales and decimal numbers (issue #60) - * [Pull request 147](https://bitbucket.org/osrf/sdformat/pull-request/147) - * [Issue 60](https://bitbucket.org/osrf/sdformat/issues/60) + * [BitBucket pull request 147](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/147) + * [Issue 60](https://github.com/osrf/sdformat/issues/60) ## SDFormat 2.x 1. rename cfm_damping --> implicit_spring_damper - * [Pull request 59](https://bitbucket.org/osrf/sdformat/pull-request/59) + * [BitBucket pull request 59](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/59) 1. add gear_ratio and reference_body for gearbox joint. - * [Pull request 62](https://bitbucket.org/osrf/sdformat/pull-request/62) + * [BitBucket pull request 62](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/62) 1. Update joint stop stiffness and dissipation - * [Pull request 61](https://bitbucket.org/osrf/sdformat/pull-request/61) + * [BitBucket pull request 61](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/61) 1. Support for GNUInstallDirs - * [Pull request 64](https://bitbucket.org/osrf/sdformat/pull-request/64) + * [BitBucket pull request 64](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/64) 1. `` element used by DEM heightmaps - * [Pull request 67](https://bitbucket.org/osrf/sdformat/pull-request/67) + * [BitBucket pull request 67](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/67) 1. Do not export urdf symbols in sdformat 1.4 - * [Pull request 75](https://bitbucket.org/osrf/sdformat/pull-request/75) + * [BitBucket pull request 75](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/75) 1. adding deformable properties per issue #32 - * [Pull request 78](https://bitbucket.org/osrf/sdformat/pull-request/78) - * [Issue 32](https://bitbucket.org/osrf/sdformat/issues/32) + * [BitBucket pull request 78](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/78) + * [Issue 32](https://github.com/osrf/sdformat/issues/32) 1. Support to use external URDF - * [Pull request 77](https://bitbucket.org/osrf/sdformat/pull-request/77) + * [BitBucket pull request 77](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/77) 1. Add lighting element to visual - * [Pull request 79](https://bitbucket.org/osrf/sdformat/pull-request/79) + * [BitBucket pull request 79](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/79) 1. SDF 1.5: add flag to fix joint axis frame #43 (gazebo issue 494) - * [Pull request 83](https://bitbucket.org/osrf/sdformat/pull-request/83) - * [Issue 43](https://bitbucket.org/osrf/sdformat/issues/43) - * [Gazebo issue 494](https://bitbucket.org/osrf/gazebo/issues/494) + * [BitBucket pull request 83](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/83) + * [Issue 43](https://github.com/osrf/sdformat/issues/43) + * [Gazebo issue 494](https://github.com/osrf/gazebo/issues/494) 1. Implement SDF_PROTOCOL_VERSION (issue #51) - * [Pull request 90](https://bitbucket.org/osrf/sdformat/pull-request/90) + * [BitBucket pull request 90](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/90) 1. Port sdformat to compile on Windows (MSVC) - * [Pull request 101](https://bitbucket.org/osrf/sdformat/pull-request/101) + * [BitBucket pull request 101](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/101) 1. Separate material properties in material.sdf - * [Pull request 104](https://bitbucket.org/osrf/sdformat/pull-request/104) + * [BitBucket pull request 104](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/104) 1. Add road textures (repeat pull request #104 for sdf_2.0) - * [Pull request 105](https://bitbucket.org/osrf/sdformat/pull-request/105) + * [BitBucket pull request 105](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/105) 1. Add Extruded Polylines as a model - * [Pull request 93](https://bitbucket.org/osrf/sdformat/pull-request/93) + * [BitBucket pull request 93](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/93) 1. Added polyline for sdf_2.0 - * [Pull request 106](https://bitbucket.org/osrf/sdformat/pull-request/106) + * [BitBucket pull request 106](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/106) 1. Add spring_reference and spring_stiffness tags to joint axis dynamics - * [Pull request 102](https://bitbucket.org/osrf/sdformat/pull-request/102) + * [BitBucket pull request 102](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/102) 1. Fix actor static - * [Pull request 110](https://bitbucket.org/osrf/sdformat/pull-request/110) + * [BitBucket pull request 110](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/110) 1. New element - * [Pull request 112](https://bitbucket.org/osrf/sdformat/pull-request/112) + * [BitBucket pull request 112](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/112) 1. Add camera distortion element - * [Pull request 120](https://bitbucket.org/osrf/sdformat/pull-request/120) + * [BitBucket pull request 120](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/120) 1. Inclusion of magnetic field strength sensor - * [Pull request 123](https://bitbucket.org/osrf/sdformat/pull-request/123) + * [BitBucket pull request 123](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/123) 1. Properly add URDF gazebo extensions blobs to SDF joint elements - * [Pull request 125](https://bitbucket.org/osrf/sdformat/pull-request/125) + * [BitBucket pull request 125](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/125) 1. Allow gui plugins to be specified in SDF - * [Pull request 127](https://bitbucket.org/osrf/sdformat/pull-request/127) + * [BitBucket pull request 127](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/127) 1. Backport magnetometer - * [Pull request 128](https://bitbucket.org/osrf/sdformat/pull-request/128) + * [BitBucket pull request 128](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/128) 1. Add flag for MOI rescaling to sdf 1.4 - * [Pull request 121](https://bitbucket.org/osrf/sdformat/pull-request/121) + * [BitBucket pull request 121](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/121) 1. Parse urdf joint friction parameters, add corresponding test - * [Pull request 129](https://bitbucket.org/osrf/sdformat/pull-request/129) + * [BitBucket pull request 129](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/129) 1. Allow reading of boolean values from plugin elements. - * [Pull request 132](https://bitbucket.org/osrf/sdformat/pull-request/132) + * [BitBucket pull request 132](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/132) 1. Implement generation of XML Schema files (issue #2) - * [Pull request 91](https://bitbucket.org/osrf/sdformat/pull-request/91) + * [BitBucket pull request 91](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/91) 1. Fix build for OS X 10.10 - * [Pull request 135](https://bitbucket.org/osrf/sdformat/pull-request/135) + * [BitBucket pull request 135](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/135) 1. Improve performance in loading SDF elements - * [Pull request 138](https://bitbucket.org/osrf/sdformat/pull-request/138) + * [BitBucket pull request 138](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/138) 1. Added urdf gazebo extension option to disable fixed joint lumping - * [Pull request 133](https://bitbucket.org/osrf/sdformat/pull-request/133) + * [BitBucket pull request 133](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/133) 1. Support urdfdom 0.3 (alternative to pull request #122) - * [Pull request 141](https://bitbucket.org/osrf/sdformat/pull-request/141) + * [BitBucket pull request 141](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/141) 1. Update list of supported joint types - * [Pull request 142](https://bitbucket.org/osrf/sdformat/pull-request/142) + * [BitBucket pull request 142](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/142) 1. Ignore unknown elements - * [Pull request 148](https://bitbucket.org/osrf/sdformat/pull-request/148) + * [BitBucket pull request 148](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/148) 1. Physics preset attributes - * [Pull request 146](https://bitbucket.org/osrf/sdformat/pull-request/146) + * [BitBucket pull request 146](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/146) 1. Backport fix for latin locales (pull request #147) - * [Pull request 150](https://bitbucket.org/osrf/sdformat/pull-request/150) + * [BitBucket pull request 150](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/150) ## SDFormat 1.4 ### SDFormat 1.4.8 (2013-09-06) 1. Fix inertia transformations when reducing fixed joints in URDF - * [Pull request 48](https://bitbucket.org/osrf/sdformat/pull-request/48/fix-for-issue-22-reducing-inertia-across/diff) + * [BitBucket pull request 48](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/48/fix-for-issue-22-reducing-inertia-across/diff) 1. Add element to support terrain paging in gazebo - * [Pull request 47](https://bitbucket.org/osrf/sdformat/pull-request/47/add-element-inside-heightmap/diff) + * [BitBucket pull request 47](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/47/add-element-inside-heightmap/diff) 1. Further reduce console output when using URDF models - * [Pull request 46](https://bitbucket.org/osrf/sdformat/pull-request/46/convert-a-few-more-sdfwarns-to-sdflog-fix/diff) - * [Commit](https://bitbucket.org/osrf/sdformat/commits/b15d5a1ecc57abee6691618d02d59bbc3d1b84dc) + * [BitBucket pull request 46](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/46/convert-a-few-more-sdfwarns-to-sdflog-fix/diff) + * [Commit](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/commits/b15d5a1ecc57abee6691618d02d59bbc3d1b84dc) ### SDFormat 1.4.7 (2013-08-22) 1. Direct console messages to std_err - * [Pull request 44](https://bitbucket.org/osrf/sdformat/pull-request/44/fix-19-direct-all-messages-to-std_err) + * [BitBucket pull request 44](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/44/fix-19-direct-all-messages-to-std_err) ### SDFormat 1.4.6 (2013-08-20) 1. Add tags for GPS sensor and sensor noise - * [Pull request 36](https://bitbucket.org/osrf/sdformat/pull-request/36/gps-sensor-sensor-noise-and-spherical) + * [BitBucket pull request 36](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/36/gps-sensor-sensor-noise-and-spherical) 1. Add tags for wireless transmitter/receiver models - * [Pull request 34](https://bitbucket.org/osrf/sdformat/pull-request/34/transceiver-support) - * [Pull request 43](https://bitbucket.org/osrf/sdformat/pull-request/43/updated-description-of-the-transceiver-sdf) + * [BitBucket pull request 34](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/34/transceiver-support) + * [BitBucket pull request 43](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/43/updated-description-of-the-transceiver-sdf) 1. Add tags for playback of audio files in Gazebo - * [Pull request 26](https://bitbucket.org/osrf/sdformat/pull-request/26/added-audio-tags) - * [Pull request 35](https://bitbucket.org/osrf/sdformat/pull-request/35/move-audio-to-link-and-playback-on-contact) + * [BitBucket pull request 26](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/26/added-audio-tags) + * [BitBucket pull request 35](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/35/move-audio-to-link-and-playback-on-contact) 1. Add tags for simbody physics parameters - * [Pull request 32](https://bitbucket.org/osrf/sdformat/pull-request/32/merging-some-updates-from-simbody-branch) + * [BitBucket pull request 32](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/32/merging-some-updates-from-simbody-branch) 1. Log messages to a file, reduce console output - * [Pull request 33](https://bitbucket.org/osrf/sdformat/pull-request/33/log-messages-to-file-8) + * [BitBucket pull request 33](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/33/log-messages-to-file-8) 1. Generalize ode's element - * [Pull request 38](https://bitbucket.org/osrf/sdformat/pull-request/38/add-provide_feedback-for-bullet-joint) + * [BitBucket pull request 38](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/38/add-provide_feedback-for-bullet-joint) 1. Various bug, style and test fixes ### SDFormat 1.4.5 (2013-07-23) diff --git a/INSTALL_WIN32.md b/INSTALL_WIN32.md index 491fd744c..2d9018b00 100644 --- a/INSTALL_WIN32.md +++ b/INSTALL_WIN32.md @@ -26,7 +26,7 @@ Windows `cmd` for configuring and building. 1. Clone sdformat - hg clone https://bitbucket.org/osrf/sdformat + git clone https://github.com/osrf/sdformat 1. Load your compiler setup, e.g. (note that we are asking for the 64-bit toolchain here): diff --git a/Migration.md b/Migration.md index 9b7fe28e9..3f2622c1b 100644 --- a/Migration.md +++ b/Migration.md @@ -75,22 +75,22 @@ but with improved human-readability.. 1. **`gravity` and `magnetic_field` elements are moved from `physics` to `world`** + In physics element: gravity and magnetic_field tags have been moved from Physics to World element. - + [pull request 247](https://bitbucket.org/osrf/sdformat/pull-requests/247) - + [gazebo pull request 2090](https://bitbucket.org/osrf/gazebo/pull-requests/2090) + + [BitBucket pull request 247](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/247) + + [gazebo pull request 2090](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/2090) 1. **New noise for IMU** + A new style for representing the noise properties of an `imu` was implemented - in [pull request 199](https://bitbucket.org/osrf/sdformat/pull-requests/199) + in [BitBucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) for sdf 1.5 and the old style was declared as deprecated. The old style has been removed from sdf 1.6 with the conversion script updating to the new style. - + [pull request 199](https://bitbucket.org/osrf/sdformat/pull-requests/199) - + [pull request 243](https://bitbucket.org/osrf/sdformat/pull-requests/243) - + [pull request 244](https://bitbucket.org/osrf/sdformat/pull-requests/244) + + [BitBucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) + + [BitBucket pull request 243](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/243) + + [BitBucket pull request 244](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/244) 1. **Lump:: prefix in link names** + Changed to \_fixed_joint_lump__ to avoid confusion with scoped names - + [Pull request 245](https://bitbucket.org/osrf/sdformat/pull-request/245) + + [BitBucket pull request 245](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/245) ## SDF protocol 1.5 to 1.6 @@ -106,23 +106,23 @@ but with improved human-readability.. + min: 0.0 + max: 1.0 + required: 0 - + [pull request 466](https://bitbucket.org/osrf/sdformat/pull-requests/466) + + [BitBucket pull request 466](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/466) 1. **camera.sdf** `intrinsics` sub-elements: `fx`, `fy`, `cx`, `cy`, `s` + description: Camera intrinsic parameters for setting a custom perspective projection matrix. - + [pull request 496](https://bitbucket.org/osrf/sdformat/pull-requests/496) + + [BitBucket pull request 496](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/496) 1. **link.sdf** `enable_wind` element + description: If true, the link is affected by the wind + type: bool + default: false + required: 0 - + [pull request 240](https://bitbucket.org/osrf/sdformat/pull-requests/240) + + [BitBucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) 1. **link.sdf** `light` element + included from `light.sdf` with required="*", so a link can have any number of attached lights. - + [pull request 373](https://bitbucket.org/osrf/sdformat/pull-requests/373) + + [BitBucket pull request 373](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/373) 1. **model.sdf** `enable_wind` element + description: If set to true, all links in the model will be affected by @@ -130,14 +130,14 @@ but with improved human-readability.. + type: bool + default: false + required: 0 - + [pull request 240](https://bitbucket.org/osrf/sdformat/pull-requests/240) + + [BitBucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) 1. **model_state.sdf** `scale` element + description: Scale for the 3 dimensions of the model. + type: vector3 + default: "1 1 1" + required: 0 - + [pull request 246](https://bitbucket.org/osrf/sdformat/pull-requests/246) + + [BitBucket pull request 246](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/246) 1. **physics.sdf** `dart::collision_detector` element + description: The collision detector for DART to use. @@ -145,7 +145,7 @@ but with improved human-readability.. + type: string + default: fcl + required: 0 - + [pull request 440](https://bitbucket.org/osrf/sdformat/pull-requests/440) + + [BitBucket pull request 440](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/440) 1. **physics.sdf** `dart::solver::solver_type` element + description: The DART LCP/constraint solver to use. @@ -153,31 +153,31 @@ but with improved human-readability.. + type: string + default: dantzig + required: 0 - + [pull request 369](https://bitbucket.org/osrf/sdformat/pull-requests/369) + + [BitBucket pull request 369](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/369) 1. **physics.sdf** `island_threads` element under `ode::solver` + description: Number of threads to use for "islands" of disconnected models. + type: int + default: 0 + required: 0 - + [pull request 380](https://bitbucket.org/osrf/sdformat/pull-requests/380) + + [BitBucket pull request 380](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/380) 1. **physics.sdf** `thread_position_correction` element under `ode::solver` + description: Flag to use threading to speed up position correction computation. + type: bool + default: 0 + required: 0 - + [pull request 380](https://bitbucket.org/osrf/sdformat/pull-requests/380) + + [BitBucket pull request 380](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/380) 1. **sonar.sdf** `geometry` element + description: The sonar collision shape. Currently supported geometries are: "cone" and "sphere". + type: string + default: "cone" + required: 0 - + [pull request 495](https://bitbucket.org/osrf/sdformat/pull-requests/495) + + [BitBucket pull request 495](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/495) 1. **state.sdf** allow `light` tags within `insertions` element - * [pull request 325](https://bitbucket.org/osrf/sdformat/pull-request/325) + * [BitBucket pull request 325](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/325) 1. **surface.sdf** `category_bitmask` element + description: Bitmask for category of collision filtering. @@ -186,16 +186,16 @@ but with improved human-readability.. + type: unsigned int + default: 65535 + required: 0 - + [pull request 318](https://bitbucket.org/osrf/sdformat/pull-requests/318) + + [BitBucket pull request 318](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/318) 1. **world.sdf** `wind` element + description: The wind tag specifies the type and properties of the wind. + required: 0 - + [pull request 240](https://bitbucket.org/osrf/sdformat/pull-requests/240) + + [BitBucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) 1. **world.sdf** `wind::linear_velocity` element + description: Linear velocity of the wind. + type: vector3 + default: "0 0 0" + required: 0 - + [pull request 240](https://bitbucket.org/osrf/sdformat/pull-requests/240) + + [BitBucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index dc9e65465..1b04f6ee5 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -6,7 +6,7 @@ pipelines: script: # Dependencies - apt update - - apt -y install cmake build-essential lcov curl mercurial lsb-release wget + - apt -y install cmake build-essential lcov curl git lsb-release wget libtinyxml-dev libxml2-utils ruby-dev libboost-dev python-psutil - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-latest.list' diff --git a/doc/header.html b/doc/header.html index d4b88ef24..f2e06b44a 100644 --- a/doc/header.html +++ b/doc/header.html @@ -41,7 +41,7 @@

Tutorials
Download
--> -
Report Documentation Issues
+
Report Documentation Issues
diff --git a/doc/mainpage.html b/doc/mainpage.html index 11ed37316..404390775 100644 --- a/doc/mainpage.html +++ b/doc/mainpage.html @@ -5,7 +5,7 @@ Desctiption Format API. The code reference is divided into the groups below. Should you find problems with this documentation - typos, unclear phrases, or insufficient detail - please create a new bitbucket issue. + href="https://github.com/osrf/sdf/issues/new">new GitHub issue. Include sufficient detail to quickly locate the problematic documentation, and set the issue's fields accordingly: Assignee - blank; Kind - bug; Priority - minor; Version - blank. From 530a85626303a605dc3e08508381ecdf079bc11c Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Wed, 29 Apr 2020 15:07:53 -0700 Subject: [PATCH 02/60] [sdf4] Update BitBucket links (#248) (#258) * [sdf4] Update BitBucket links Signed-off-by: Louise Poubel * fix a few more links Signed-off-by: Steven Peters Co-authored-by: Steven Peters Co-authored-by: chapulina --- .hgignore => .gitignore | 1 - Changelog.md | 232 ++++++++++++++++++++-------------------- Migration.md | 4 +- sdf/1.4/physics.sdf | 2 +- sdf/1.5/joint.sdf | 4 +- sdf/1.5/physics.sdf | 2 +- sdf/1.6/joint.sdf | 4 +- sdf/1.6/physics.sdf | 6 +- sdf/Migration.md | 30 +++--- src/Param_TEST.cc | 4 +- 10 files changed, 144 insertions(+), 145 deletions(-) rename .hgignore => .gitignore (63%) diff --git a/.hgignore b/.gitignore similarity index 63% rename from .hgignore rename to .gitignore index 1b7892bf6..416ecfb5f 100644 --- a/.hgignore +++ b/.gitignore @@ -1,4 +1,3 @@ -syntax: glob build build_* diff --git a/Changelog.md b/Changelog.md index 7763b968f..9b5c91a0f 100644 --- a/Changelog.md +++ b/Changelog.md @@ -175,24 +175,24 @@ ### SDFormat 4.4.0 (2017-10-26) 1. Add ODE parallelization parameters: threaded islands and position correction - * [BitBucket pull request 380](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/380) + * [BitBucket pull request 380](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/380) 1. surface.sdf: expand documentation of friction and slip coefficients - * [BitBucket pull request 343](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/343) + * [BitBucket pull request 343](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/343) 1. Add preserveFixedJoint option to the URDF parser - * [BitBucket pull request 352](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/352) + * [BitBucket pull request 352](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/352) 1. Add light as child of link - * [BitBucket pull request 373](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/373) + * [BitBucket pull request 373](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/373) ### SDFormat 4.3.2 (2017-07-19) 1. Add documentation for `Element::GetFirstElement()` and `Element::GetNextElement()` - * [BitBucket pull request 341](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/341) + * [BitBucket pull request 341](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/341) 1. Fix parser to read plugin child elements within an `` - * [BitBucket pull request 350](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/350) + * [BitBucket pull request 350](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/350) ### SDFormat 4.3.1 (2017-03-24) @@ -203,83 +203,83 @@ ### SDFormat 4.3.0 (2017-03-20) 1. Choosing models with more recent sdf version with `` tag - * [BitBucket pull request 291](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/291) + * [BitBucket pull request 291](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/291) * [Issue 123](https://github.com/osrf/sdformat/issues/123) 1. Added `` to 1.6 surface contact parameters - * [BitBucket pull request 318](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/318) + * [BitBucket pull request 318](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/318) 1. Support light insertion in state - * [BitBucket pull request 325](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/325) + * [BitBucket pull request 325](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/325) 1. Case insensitive boolean strings - * [BitBucket pull request 322](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/322) + * [BitBucket pull request 322](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/322) 1. Enable coverage testing - * [BitBucket pull request 317](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/317) + * [BitBucket pull request 317](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/317) 1. Add `friction_model` parameter to ode solver - * [BitBucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/294) - * [Gazebo pull request 1522](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1522) + * [BitBucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/294) + * [Gazebo pull request 1522](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/1522) 1. Added `sampling` parameter to `` SDF element. - * [BitBucket pull request 293](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/293) + * [BitBucket pull request 293](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/293) 1. Added Migration guide - * [BitBucket pull request 290](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/290) + * [BitBucket pull request 290](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/290) 1. Add cmake `@PKG_NAME@_LIBRARY_DIRS` variable to cmake config file - * [BitBucket pull request 292](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/292) + * [BitBucket pull request 292](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/292) ### SDFormat 4.2.0 (2016-10-10) 1. Added tag to specify ODE friction model. - * [BitBucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/294) + * [BitBucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/294) 1. Fix URDF to SDF `self_collide` bug. - * [BitBucket pull request 287](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/287) + * [BitBucket pull request 287](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/287) 1. Added IMU orientation specification to SDF. - * [BitBucket pull request 284](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/284) + * [BitBucket pull request 284](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/284) ### SDFormat 4.1.1 (2016-07-08) 1. Added documentation and animation to `` element. - * [BitBucket pull request 280](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/280) + * [BitBucket pull request 280](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/280) 1. Added tag to specify initial joint position - * [BitBucket pull request 279](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/279) + * [BitBucket pull request 279](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/279) ### SDFormat 4.1.0 (2016-04-01) 1. Added SDF conversion functions to parser including sdf::convertFile and sdf::convertString. - * [BitBucket pull request 266](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/266) + * [BitBucket pull request 266](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/266) 1. Added an upload script - * [BitBucket pull request 256](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/256) + * [BitBucket pull request 256](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/256) ### SDFormat 4.0.0 (2015-01-12) 1. Boost pointers and boost::function in the public API have been replaced by their std::equivalents (C++11 standard) 1. Move gravity and magnetic_field tags from physics to world - * [BitBucket pull request 247](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/247) + * [BitBucket pull request 247](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/247) 1. Switch lump link prefix from lump:: to lump_ - * [BitBucket pull request 245](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/245) + * [BitBucket pull request 245](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/245) 1. New element. A contribution from Olivier Crave - * [BitBucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/240) + * [BitBucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) 1. Add scale to model state - * [BitBucket pull request 246](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/246) + * [BitBucket pull request 246](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/246) 1. Use stof functions to parse hex strings as floating point params. A contribution from Rich Mattes - * [BitBucket pull request 250](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/250) + * [BitBucket pull request 250](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/250) 1. Fix memory leaks. A contribution from Silvio Traversaro - * [BitBucket pull request 249](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/249) + * [BitBucket pull request 249](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/249) 1. Update SDF to version 1.6: new style for representing the noise properties of an `imu` - * [BitBucket pull request 243](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/243) + * [BitBucket pull request 243](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/243) * [BitBucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) ## SDFormat 3.0 @@ -293,219 +293,219 @@ ### SDFormat 3.7.0 (2015-11-20) 1. Add spring pass through for sdf3 - * [Design document](https://bitbucket.org/osrf/gazebo_design/pull-requests/23) - * [BitBucket pull request 242](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/242) + * [Design document](https://github.com/osrf/gazebo_design/pull-requests/23) + * [BitBucket pull request 242](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/242) 1. Support frame specification in SDF - * [BitBucket pull request 237](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/237) + * [BitBucket pull request 237](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/237) 1. Remove boost from SDFExtension - * [BitBucket pull request 229](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/229) + * [BitBucket pull request 229](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/229) ### SDFormat 3.6.0 (2015-10-27) 1. Add light state - * [BitBucket pull request 227](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/227) + * [BitBucket pull request 227](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/227) 1. redo pull request #222 for sdf3 branch - * [BitBucket pull request 232](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/232) + * [BitBucket pull request 232](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/232) 1. Fix links in API documentation - * [BitBucket pull request 231](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/231) + * [BitBucket pull request 231](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/231) ### SDFormat 3.5.0 (2015-10-07) 1. Camera lens description (Replaces #213) - * [BitBucket pull request 215](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/215) + * [BitBucket pull request 215](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/215) 1. Fix shared pointer reference loop in Element and memory leak (#104) - * [BitBucket pull request 230](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/230) + * [BitBucket pull request 230](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/230) ### SDFormat 3.4.0 (2015-10-05) 1. Support nested model states - * [BitBucket pull request 223](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/223) + * [BitBucket pull request 223](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/223) 1. Cleaner way to set SDF_PATH for tests - * [BitBucket pull request 226](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/226) + * [BitBucket pull request 226](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/226) ### SDFormat 3.3.0 (2015-09-15) 1. Windows Boost linking errors - * [BitBucket pull request 206](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/206) + * [BitBucket pull request 206](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/206) 1. Nested SDF -> sdf3 - * [BitBucket pull request 221](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/221) + * [BitBucket pull request 221](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/221) 1. Pointer types - * [BitBucket pull request 218](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/218) + * [BitBucket pull request 218](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/218) 1. Torsional friction default surface radius not infinity - * [BitBucket pull request 217](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/217) + * [BitBucket pull request 217](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/217) ### SDFormat 3.2.2 (2015-08-24) 1. Added battery element (contribution from Olivier Crave) - * [BitBucket pull request #204](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/204) + * [BitBucket pull request #204](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/204) 1. Torsional friction backport - * [BitBucket pull request #211](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/211) + * [BitBucket pull request #211](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/211) 1. Allow Visual Studio 2015 - * [BitBucket pull request #208](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/208) + * [BitBucket pull request #208](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/208) ### SDFormat 3.1.1 (2015-08-03) 1. Fix tinyxml linking error - * [BitBucket pull request #209](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/209) + * [BitBucket pull request #209](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/209) ### SDFormat 3.1.0 (2015-08-02) 1. Added logical camera sensor to SDF - * [BitBucket pull request #207](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/207) + * [BitBucket pull request #207](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/207) ### SDFormat 3.0.0 (2015-07-24) 1. Added battery to SDF - * [BitBucket pull request 204](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/204) + * [BitBucket pull request 204](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/204) 1. Added altimeter sensor to SDF - * [BitBucket pull request #197](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/197) + * [BitBucket pull request #197](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/197) 1. Added magnetometer sensor to SDF - * [BitBucket pull request 198](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/198) + * [BitBucket pull request 198](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/198) 1. Fix detection of XML parsing errors - * [BitBucket pull request 190](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/190) + * [BitBucket pull request 190](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/190) 1. Support for fixed joints - * [BitBucket pull request 194](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/194) + * [BitBucket pull request 194](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/194) 1. Adding iterations to state - * [BitBucket pull request 188](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/188) + * [BitBucket pull request 188](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/188) 1. Convert to use ignition-math - * [BitBucket pull request 173](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/173) + * [BitBucket pull request 173](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/173) 1. Add world origin to scene - * [BitBucket pull request 183](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/183) + * [BitBucket pull request 183](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/183) 1. Fix collide bitmask - * [BitBucket pull request 182](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/182) + * [BitBucket pull request 182](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/182) 1. Adding meta information to visuals - * [BitBucket pull request 180](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/180) + * [BitBucket pull request 180](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/180) 1. Add projection type to gui camera - * [BitBucket pull request 178](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/178) + * [BitBucket pull request 178](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/178) 1. Fix print description to include attribute description - * [BitBucket pull request 170](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/170) + * [BitBucket pull request 170](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/170) 1. Add -std=c++11 flag to sdf_config.cmake.in and sdformat.pc.in, needed by downstream code - * [BitBucket pull request 172](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/172) + * [BitBucket pull request 172](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/172) 1. Added boost::any accessor for Param and Element - * [BitBucket pull request 166](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/166) + * [BitBucket pull request 166](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/166) 1. Remove tinyxml from dependency list - * [BitBucket pull request 152](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/152) + * [BitBucket pull request 152](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/152) 1. Added self_collide element for model - * [BitBucket pull request 149](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/149) + * [BitBucket pull request 149](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/149) 1. Added a collision bitmask field to sdf-1.5 and c++11 support - * [BitBucket pull request 145](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/145) + * [BitBucket pull request 145](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/145) 1. Fix problems with latin locales and decimal numbers (issue #60) - * [BitBucket pull request 147](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/147) + * [BitBucket pull request 147](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/147) * [Issue 60](https://github.com/osrf/sdformat/issues/60) ## SDFormat 2.x 1. rename cfm_damping --> implicit_spring_damper - * [BitBucket pull request 59](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/59) + * [BitBucket pull request 59](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/59) 1. add gear_ratio and reference_body for gearbox joint. - * [BitBucket pull request 62](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/62) + * [BitBucket pull request 62](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/62) 1. Update joint stop stiffness and dissipation - * [BitBucket pull request 61](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/61) + * [BitBucket pull request 61](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/61) 1. Support for GNUInstallDirs - * [BitBucket pull request 64](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/64) + * [BitBucket pull request 64](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/64) 1. `` element used by DEM heightmaps - * [BitBucket pull request 67](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/67) + * [BitBucket pull request 67](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/67) 1. Do not export urdf symbols in sdformat 1.4 - * [BitBucket pull request 75](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/75) + * [BitBucket pull request 75](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/75) 1. adding deformable properties per issue #32 - * [BitBucket pull request 78](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/78) + * [BitBucket pull request 78](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/78) * [Issue 32](https://github.com/osrf/sdformat/issues/32) 1. Support to use external URDF - * [BitBucket pull request 77](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/77) + * [BitBucket pull request 77](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/77) 1. Add lighting element to visual - * [BitBucket pull request 79](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/79) + * [BitBucket pull request 79](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/79) 1. SDF 1.5: add flag to fix joint axis frame #43 (gazebo issue 494) - * [BitBucket pull request 83](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/83) + * [BitBucket pull request 83](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/83) * [Issue 43](https://github.com/osrf/sdformat/issues/43) * [Gazebo issue 494](https://github.com/osrf/gazebo/issues/494) 1. Implement SDF_PROTOCOL_VERSION (issue #51) - * [BitBucket pull request 90](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/90) + * [BitBucket pull request 90](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/90) 1. Port sdformat to compile on Windows (MSVC) - * [BitBucket pull request 101](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/101) + * [BitBucket pull request 101](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/101) 1. Separate material properties in material.sdf - * [BitBucket pull request 104](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/104) + * [BitBucket pull request 104](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/104) 1. Add road textures (repeat pull request #104 for sdf_2.0) - * [BitBucket pull request 105](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/105) + * [BitBucket pull request 105](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/105) 1. Add Extruded Polylines as a model - * [BitBucket pull request 93](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/93) + * [BitBucket pull request 93](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/93) 1. Added polyline for sdf_2.0 - * [BitBucket pull request 106](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/106) + * [BitBucket pull request 106](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/106) 1. Add spring_reference and spring_stiffness tags to joint axis dynamics - * [BitBucket pull request 102](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/102) + * [BitBucket pull request 102](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/102) 1. Fix actor static - * [BitBucket pull request 110](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/110) + * [BitBucket pull request 110](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/110) 1. New element - * [BitBucket pull request 112](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/112) + * [BitBucket pull request 112](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/112) 1. Add camera distortion element - * [BitBucket pull request 120](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/120) + * [BitBucket pull request 120](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/120) 1. Inclusion of magnetic field strength sensor - * [BitBucket pull request 123](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/123) + * [BitBucket pull request 123](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/123) 1. Properly add URDF gazebo extensions blobs to SDF joint elements - * [BitBucket pull request 125](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/125) + * [BitBucket pull request 125](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/125) 1. Allow gui plugins to be specified in SDF - * [BitBucket pull request 127](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/127) + * [BitBucket pull request 127](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/127) 1. Backport magnetometer - * [BitBucket pull request 128](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/128) + * [BitBucket pull request 128](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/128) 1. Add flag for MOI rescaling to sdf 1.4 - * [BitBucket pull request 121](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/121) + * [BitBucket pull request 121](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/121) 1. Parse urdf joint friction parameters, add corresponding test - * [BitBucket pull request 129](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/129) + * [BitBucket pull request 129](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/129) 1. Allow reading of boolean values from plugin elements. - * [BitBucket pull request 132](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/132) + * [BitBucket pull request 132](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/132) 1. Implement generation of XML Schema files (issue #2) - * [BitBucket pull request 91](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/91) + * [BitBucket pull request 91](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/91) 1. Fix build for OS X 10.10 - * [BitBucket pull request 135](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/135) + * [BitBucket pull request 135](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/135) 1. Improve performance in loading SDF elements - * [BitBucket pull request 138](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/138) + * [BitBucket pull request 138](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/138) 1. Added urdf gazebo extension option to disable fixed joint lumping - * [BitBucket pull request 133](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/133) + * [BitBucket pull request 133](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/133) 1. Support urdfdom 0.3 (alternative to pull request #122) - * [BitBucket pull request 141](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/141) + * [BitBucket pull request 141](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/141) 1. Update list of supported joint types - * [BitBucket pull request 142](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/142) + * [BitBucket pull request 142](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/142) 1. Ignore unknown elements - * [BitBucket pull request 148](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/148) + * [BitBucket pull request 148](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/148) 1. Physics preset attributes - * [BitBucket pull request 146](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/146) + * [BitBucket pull request 146](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/146) 1. Backport fix for latin locales (pull request #147) - * [BitBucket pull request 150](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/150) + * [BitBucket pull request 150](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/150) ## SDFormat 1.4 ### SDFormat 1.4.8 (2013-09-06) 1. Fix inertia transformations when reducing fixed joints in URDF - * [BitBucket pull request 48](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/48/fix-for-issue-22-reducing-inertia-across/diff) + * [BitBucket pull request 48](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/48/fix-for-issue-22-reducing-inertia-across/diff) 1. Add element to support terrain paging in gazebo - * [BitBucket pull request 47](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/47/add-element-inside-heightmap/diff) + * [BitBucket pull request 47](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/47/add-element-inside-heightmap/diff) 1. Further reduce console output when using URDF models - * [BitBucket pull request 46](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/46/convert-a-few-more-sdfwarns-to-sdflog-fix/diff) + * [BitBucket pull request 46](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/46/convert-a-few-more-sdfwarns-to-sdflog-fix/diff) * [Commit](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/commits/b15d5a1ecc57abee6691618d02d59bbc3d1b84dc) ### SDFormat 1.4.7 (2013-08-22) 1. Direct console messages to std_err - * [BitBucket pull request 44](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/44/fix-19-direct-all-messages-to-std_err) + * [BitBucket pull request 44](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/44/fix-19-direct-all-messages-to-std_err) ### SDFormat 1.4.6 (2013-08-20) 1. Add tags for GPS sensor and sensor noise - * [BitBucket pull request 36](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/36/gps-sensor-sensor-noise-and-spherical) + * [BitBucket pull request 36](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/36/gps-sensor-sensor-noise-and-spherical) 1. Add tags for wireless transmitter/receiver models - * [BitBucket pull request 34](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/34/transceiver-support) - * [BitBucket pull request 43](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/43/updated-description-of-the-transceiver-sdf) + * [BitBucket pull request 34](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/34/transceiver-support) + * [BitBucket pull request 43](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/43/updated-description-of-the-transceiver-sdf) 1. Add tags for playback of audio files in Gazebo - * [BitBucket pull request 26](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/26/added-audio-tags) - * [BitBucket pull request 35](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/35/move-audio-to-link-and-playback-on-contact) + * [BitBucket pull request 26](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/26/added-audio-tags) + * [BitBucket pull request 35](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/35/move-audio-to-link-and-playback-on-contact) 1. Add tags for simbody physics parameters - * [BitBucket pull request 32](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/32/merging-some-updates-from-simbody-branch) + * [BitBucket pull request 32](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/32/merging-some-updates-from-simbody-branch) 1. Log messages to a file, reduce console output - * [BitBucket pull request 33](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/33/log-messages-to-file-8) + * [BitBucket pull request 33](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/33/log-messages-to-file-8) 1. Generalize ode's element - * [BitBucket pull request 38](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/38/add-provide_feedback-for-bullet-joint) + * [BitBucket pull request 38](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/38/add-provide_feedback-for-bullet-joint) 1. Various bug, style and test fixes ### SDFormat 1.4.5 (2013-07-23) diff --git a/Migration.md b/Migration.md index 3f2622c1b..752c94b1e 100644 --- a/Migration.md +++ b/Migration.md @@ -90,7 +90,7 @@ but with improved human-readability.. 1. **Lump:: prefix in link names** + Changed to \_fixed_joint_lump__ to avoid confusion with scoped names - + [BitBucket pull request 245](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/245) + + [BitBucket pull request 245](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/245) ## SDF protocol 1.5 to 1.6 @@ -177,7 +177,7 @@ but with improved human-readability.. + [BitBucket pull request 495](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/495) 1. **state.sdf** allow `light` tags within `insertions` element - * [BitBucket pull request 325](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-request/325) + * [BitBucket pull request 325](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/325) 1. **surface.sdf** `category_bitmask` element + description: Bitmask for category of collision filtering. diff --git a/sdf/1.4/physics.sdf b/sdf/1.4/physics.sdf index 2cb836fa1..2f50db2e7 100644 --- a/sdf/1.4/physics.sdf +++ b/sdf/1.4/physics.sdf @@ -177,7 +177,7 @@ Flag to enable dynamic rescaling of moment of inertia in constrained directions. See gazebo pull request 1114 for the implementation of this feature. - https://bitbucket.org/osrf/gazebo/pull-request/1114 + https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1114 diff --git a/sdf/1.5/joint.sdf b/sdf/1.5/joint.sdf index abc97508f..fa8ad618a 100644 --- a/sdf/1.5/joint.sdf +++ b/sdf/1.5/joint.sdf @@ -55,7 +55,7 @@ Flag to interpret the axis xyz element in the parent model frame instead of joint frame. Provided for Gazebo compatibility - (see https://bitbucket.org/osrf/gazebo/issue/494 ). + (see https://github.com/osrf/gazebo/issue/494 ). @@ -114,7 +114,7 @@ Flag to interpret the axis xyz element in the parent model frame instead of joint frame. Provided for Gazebo compatibility - (see https://bitbucket.org/osrf/gazebo/issue/494 ). + (see https://github.com/osrf/gazebo/issue/494 ). diff --git a/sdf/1.5/physics.sdf b/sdf/1.5/physics.sdf index f4f0bf918..3e065e76e 100644 --- a/sdf/1.5/physics.sdf +++ b/sdf/1.5/physics.sdf @@ -189,7 +189,7 @@ Flag to enable dynamic rescaling of moment of inertia in constrained directions. See gazebo pull request 1114 for the implementation of this feature. - https://bitbucket.org/osrf/gazebo/pull-request/1114 + https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1114 diff --git a/sdf/1.6/joint.sdf b/sdf/1.6/joint.sdf index 75e3e6388..724e598cd 100644 --- a/sdf/1.6/joint.sdf +++ b/sdf/1.6/joint.sdf @@ -60,7 +60,7 @@ Flag to interpret the axis xyz element in the parent model frame instead of joint frame. Provided for Gazebo compatibility - (see https://bitbucket.org/osrf/gazebo/issue/494 ). + (see https://github.com/osrf/gazebo/issue/494 ). @@ -124,7 +124,7 @@ Flag to interpret the axis xyz element in the parent model frame instead of joint frame. Provided for Gazebo compatibility - (see https://bitbucket.org/osrf/gazebo/issue/494 ). + (see https://github.com/osrf/gazebo/issue/494 ). diff --git a/sdf/1.6/physics.sdf b/sdf/1.6/physics.sdf index e0277e4a2..38c22f633 100644 --- a/sdf/1.6/physics.sdf +++ b/sdf/1.6/physics.sdf @@ -200,7 +200,7 @@ Flag to enable dynamic rescaling of moment of inertia in constrained directions. See gazebo pull request 1114 for the implementation of this feature. - https://bitbucket.org/osrf/gazebo/pull-request/1114 + https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1114 @@ -213,8 +213,8 @@ cone_model: friction force magnitude limited in proportion to normal force. See gazebo pull request 1522 for the implementation of this feature. - https://bitbucket.org/osrf/gazebo/pull-request/1522 - https://bitbucket.org/osrf/gazebo/commits/8c05ad64967c + https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1522 + https://github.com/osrf/gazebo/commit/968dccafdfbfca09c9b3326f855612076fed7e6f diff --git a/sdf/Migration.md b/sdf/Migration.md index cdc63b360..a6e0f945c 100644 --- a/sdf/Migration.md +++ b/sdf/Migration.md @@ -24,14 +24,14 @@ but with improved human-readability. + type: unsigned int + default: 2 + required: 0 - + [pull request 293](https://bitbucket.org/osrf/sdformat/pull-requests/293) + + [Bitbucket pull request 293](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/293) 1. **link.sdf** `enable_wind` element + description: If true, the link is affected by the wind + type: bool + default: false + required: 0 - + [pull request 240](https://bitbucket.org/osrf/sdformat/pull-requests/240) + + [Bitbucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) 1. **model.sdf** `enable_wind` element + description: If set to true, all links in the model @@ -40,14 +40,14 @@ but with improved human-readability. + type: bool + default: false + required: 0 - + [pull request 240](https://bitbucket.org/osrf/sdformat/pull-requests/240) + + [Bitbucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) 1. **model_state.sdf** `scale` element + description: Scale for the 3 dimensions of the model. + type: vector3 + default: "1 1 1" + required: 0 - + [pull request 246](https://bitbucket.org/osrf/sdformat/pull-requests/246) + + [Bitbucket pull request 246](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/246) 1. **physics.sdf** `friction_model` element + description: Name of ODE friction model to use. Valid values include: @@ -55,39 +55,39 @@ but with improved human-readability. in proportion to normal force. + box_model: friction forces limited to constant in two directions. + cone_model: friction force magnitude limited in proportion to normal force. - See [gazebo pull request 1522](https://bitbucket.org/osrf/gazebo/pull-request/1522) - (merged in [gazebo 8c05ad64967c](https://bitbucket.org/osrf/gazebo/commits/8c05ad64967c)) + See [gazebo pull request 1522](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1522) + (merged in [gazebo 8c05ad64967c](https://github.com/osrf/gazebo/commit/968dccafdfbfca09c9b3326f855612076fed7e6f)) for the implementation of this feature. + type: string + default: "pyramid_model" + required: 0 - + [pull request 294](https://bitbucket.org/osrf/sdformat/pull-requests/294) + + [Bitbucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/294) 1. **world.sdf** `wind` element + description: The wind tag specifies the type and properties of the wind. + required: 0 - + [pull request 240](https://bitbucket.org/osrf/sdformat/pull-requests/240) + + [Bitbucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) 1. **world.sdf** `wind::linear_velocity` element + description: Linear velocity of the wind. + type: vector3 + default: "0 0 0" + required: 0 - + [pull request 240](https://bitbucket.org/osrf/sdformat/pull-requests/240) + + [Bitbucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) ### Modifications 1. `gravity` and `magnetic_field` elements are moved from `physics` to `world` - + [pull request 247](https://bitbucket.org/osrf/sdformat/pull-requests/247) - + [gazebo pull request 2090](https://bitbucket.org/osrf/gazebo/pull-requests/2090) + + [Bitbucket pull request 247](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/247) + + [gazebo pull request 2090](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/2090) 1. A new style for representing the noise properties of an `imu` was implemented - in [pull request 199](https://bitbucket.org/osrf/sdformat/pull-requests/199) + in [Bitbucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) for sdf 1.5 and the old style was declared as deprecated. The old style has been removed from sdf 1.6 with the conversion script updating to the new style. - + [pull request 199](https://bitbucket.org/osrf/sdformat/pull-requests/199) - + [pull request 243](https://bitbucket.org/osrf/sdformat/pull-requests/243) - + [pull request 244](https://bitbucket.org/osrf/sdformat/pull-requests/244) + + [Bitbucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) + + [Bitbucket pull request 243](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/243) + + [Bitbucket pull request 244](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/244) diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc index 4ba3571c9..8f9a49396 100644 --- a/src/Param_TEST.cc +++ b/src/Param_TEST.cc @@ -158,7 +158,7 @@ TEST(Param, HexUInt) TEST(Param, HexFloat) { // Microsoft does not parse hex values properly. -// https://bitbucket.org/osrf/sdformat/issues/114 +// https://github.com/osrf/sdformat/issues/114 #ifndef _MSC_VER sdf::Param floatParam("key", "float", "0", false, "description"); float value; @@ -193,7 +193,7 @@ TEST(Param, HexDouble) EXPECT_DOUBLE_EQ(value, 0.0); // Microsoft does not parse hex values properly. -// https://bitbucket.org/osrf/sdformat/issues/114 +// https://github.com/osrf/sdformat/issues/114 #ifndef _MSC_VER EXPECT_TRUE(doubleParam.SetFromString("0x01")); EXPECT_TRUE(doubleParam.Get(value)); From 40ded2a5725b5ad6ca30cdd7ae9b6d54fdd6dc6b Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Thu, 11 Jun 2020 15:59:57 +0200 Subject: [PATCH 03/60] Patch popen/pclose method for Windows Signed-off-by: Jose Luis Rivero --- src/ign_TEST.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index dc21740dd..2ea18410a 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -23,6 +23,11 @@ #include "sdf/sdf_config.h" #include "test_config.h" +#ifdef _WIN32 + #define popen _popen + #define pclose _pclose +#endif + static const std::string g_sdfVersion(" --force-version " + std::string(SDF_VERSION_FULL)); static const std::string g_ignCommand(std::string(IGN_PATH) + "/ign"); From 38480665de9f13d4fe0328ac39f35863f3db437d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 16 Mar 2021 15:27:24 -0700 Subject: [PATCH 04/60] Parse rpyOffset as radians (#497) Signed-off-by: Ian Chen --- src/parser_urdf.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 5380da09f..39774457f 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3467,7 +3467,7 @@ void ReduceSDFExtensionPluginFrameReplace( TiXmlNode* rpyKey = (*_blobIt)->FirstChild("rpyOffset"); if (rpyKey) { - urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0); + urdf::Vector3 rpy = ParseVector3(rpyKey); _reductionTransform.Rot() = ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z); // remove xyzOffset and rpyOffset From 3571e626f49edc33cfa363a03b60f099e5f66f55 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 7 Jun 2021 14:01:41 -0700 Subject: [PATCH 05/60] Merge pull request #580 from osrf/sdf6_pr606_backport Backport sdf6: Move recursiveSameTypeUniqueNames from ign.cc to parser.cc --- Changelog.md | 3 +++ Migration.md | 5 +++++ include/sdf/parser.hh | 9 +++++++++ src/ign.cc | 35 +---------------------------------- src/parser.cc | 27 +++++++++++++++++++++++++++ src/parser_TEST.cc | 29 ++++++++++++++++++++++++++++- 6 files changed, 73 insertions(+), 35 deletions(-) diff --git a/Changelog.md b/Changelog.md index 9b5c91a0f..6c13d1c5a 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,9 @@ ### SDFormat 6.X.X (20XX-XX-XX) +1. Move recursiveSameTypeUniqueNames from ign.cc to parser.cc and make public. + * [Pull request 580](https://github.com/osrf/sdformat/pull/580) + 1. Parse urdf files to sdf 1.5 instead of 1.4 to avoid `use_parent_model_frame`. * [BitBucket pull request 575](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/575) diff --git a/Migration.md b/Migration.md index 752c94b1e..59b70dc8f 100644 --- a/Migration.md +++ b/Migration.md @@ -14,6 +14,11 @@ but with improved human-readability.. ## SDFormat 5.x to 6.x +### Additions + +1. **sdf/parser.hh** + + bool recursiveSameTypeUniqueNames(sdf::ElementPtr) + ### Deprecations 1. **sdf/Types.hh** diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index 1af5c4373..2884ba688 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -141,5 +141,14 @@ namespace sdf SDFORMAT_VISIBLE bool convertString(const std::string &_sdfString, const std::string &_version, SDFPtr _sdf); + + /// \brief Check that all sibling elements of the same type have unique names. + /// This checks recursively and should check the files exhaustively + /// rather than terminating early when the first duplicate name is found. + /// \param[in] _elem sdf Element to check recursively. + /// \return True if all contained elements have do not share a name with + /// sibling elements of the same type. + SDFORMAT_VISIBLE + bool recursiveSameTypeUniqueNames(sdf::ElementPtr _elem); } #endif diff --git a/src/ign.cc b/src/ign.cc index 806643119..4b7169b62 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -23,39 +23,6 @@ #include "sdf/parser.hh" #include "sdf/system_util.hh" -////////////////////////////////////////////////// -/// \brief Check that all sibling elements of the same type have unique names. -/// This checks recursively and should check the files exhaustively -/// rather than terminating early when the first duplicate name is found. -/// \param[in] _elem sdf Element to check recursively. -/// \return True if all contained elements have do not share a name with -/// sibling elements of the same type. -bool recursiveSameTypeUniqueNames(sdf::ElementPtr _elem) -{ - bool result = true; - auto typeNames = _elem->GetElementTypeNames(); - for (const std::string &typeName : typeNames) - { - if (!_elem->HasUniqueChildNames(typeName)) - { - std::cerr << "Non-unique names detected in type " - << typeName << " in\n" - << _elem->ToString("") - << std::endl; - result = false; - } - } - - sdf::ElementPtr child = _elem->GetFirstElement(); - while (child) - { - result = recursiveSameTypeUniqueNames(child) && result; - child = child->GetNextElement(); - } - - return result; -} - ////////////////////////////////////////////////// // cppcheck-suppress unusedFunction extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path) @@ -80,7 +47,7 @@ extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path) return -1; } - if (!recursiveSameTypeUniqueNames(sdf->Root())) + if (!sdf::recursiveSameTypeUniqueNames(sdf->Root())) { std::cerr << "Error: non-unique names detected.\n"; return -1; diff --git a/src/parser.cc b/src/parser.cc index bdffa50ce..037798e32 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1208,4 +1208,31 @@ bool convertString(const std::string &_sdfString, const std::string &_version, return false; } + +////////////////////////////////////////////////// +bool recursiveSameTypeUniqueNames(sdf::ElementPtr _elem) +{ + bool result = true; + auto typeNames = _elem->GetElementTypeNames(); + for (const std::string &typeName : typeNames) + { + if (!_elem->HasUniqueChildNames(typeName)) + { + std::cerr << "Non-unique names detected in type " + << typeName << " in\n" + << _elem->ToString("") + << std::endl; + result = false; + } + } + + sdf::ElementPtr child = _elem->GetFirstElement(); + while (child) + { + result = recursiveSameTypeUniqueNames(child) && result; + child = child->GetNextElement(); + } + + return result; +} } diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index d49e0dca6..22250b916 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -18,9 +18,10 @@ #include #include "sdf/parser.hh" #include "sdf/Element.hh" +#include "test_config.h" ///////////////////////////////////////////////// -TEST(parser, initStringTrim) +TEST(Parser, initStringTrim) { sdf::SDFPtr sdf(new sdf::SDF()); std::ostringstream stream; @@ -48,6 +49,32 @@ TEST(parser, initStringTrim) EXPECT_TRUE(attr->GetRequired()); } +///////////////////////////////////////////////// +/// Tests whether the input sdf string satisfies the unique name criteria among +/// same types +sdf::SDFPtr InitSDF() +{ + sdf::SDFPtr sdf(new sdf::SDF()); + sdf::init(sdf); + return sdf; +} + +///////////////////////////////////////////////// +TEST(Parser, NameUniqueness) +{ + std::string pathBase = PROJECT_SOURCE_PATH; + pathBase += "/test/sdf"; + + // Check an SDF file with sibling elements of the same type (world) + // that have duplicate names. + { + std::string path = pathBase +"/world_duplicate.sdf"; + sdf::SDFPtr sdf = InitSDF(); + EXPECT_TRUE(sdf::readFile(path, sdf)); + EXPECT_FALSE(sdf::recursiveSameTypeUniqueNames(sdf->Root())); + } +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) From 4d0ca5218bf260c2d9811f58b8e14032d5d32e4c Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 7 Jun 2021 16:42:30 -0700 Subject: [PATCH 06/60] Prepare 6.3.0 release (#587) * Prepare version 6.3.0 * Add entry in Changelog for 497 --- CMakeLists.txt | 2 +- Changelog.md | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a25b421fc..911714956 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) set (SDF_PROTOCOL_VERSION 1.6) set (SDF_MAJOR_VERSION 6) -set (SDF_MINOR_VERSION 2) +set (SDF_MINOR_VERSION 3) set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) diff --git a/Changelog.md b/Changelog.md index 6c13d1c5a..eae3784b8 100644 --- a/Changelog.md +++ b/Changelog.md @@ -5,6 +5,9 @@ 1. Move recursiveSameTypeUniqueNames from ign.cc to parser.cc and make public. * [Pull request 580](https://github.com/osrf/sdformat/pull/580) +1. Parse rpyOffset as radians + * [Pull request 497](https://github.com/osrf/sdformat/pull/497) + 1. Parse urdf files to sdf 1.5 instead of 1.4 to avoid `use_parent_model_frame`. * [BitBucket pull request 575](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/575) From e17aef4392b36019c4f64337ca672864857d5676 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Wed, 30 Jun 2021 14:34:15 +0200 Subject: [PATCH 07/60] Fix flattening logic for nested model names (sdf6) (#597) * Fix flattening logic for composed names in nested models When nested models use names composed by several elements (i.e: my_model::link) these were not converted by the flattening logic inside parser.cc. The change makes the logic to work with composed names. Added a test to check that this is indeed working as expected. Signed-off-by: Jose Luis Rivero --- src/parser.cc | 32 +++++++----- .../model/nested_names_test/model.config | 14 ++++++ .../model/nested_names_test/submodel_test.sdf | 21 ++++++++ test/integration/nested_model.cc | 50 +++++++++++++++++++ 4 files changed, 105 insertions(+), 12 deletions(-) create mode 100644 test/integration/model/nested_names_test/model.config create mode 100644 test/integration/model/nested_names_test/submodel_test.sdf diff --git a/src/parser.cc b/src/parser.cc index 037798e32..bfd1973ca 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1074,11 +1074,18 @@ void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF) std::string modelName = modelPtr->Get("name"); while (elem) { - if (elem->GetName() == "link") + // protect elements that should not change the name attribute + // i.e: plugin + if ((elem->GetName() == "link") || (elem->GetName() == "joint") || + (elem->GetName() == "model")) { std::string elemName = elem->Get("name"); std::string newName = modelName + "::" + elemName; replace[elemName] = newName; + } + + if (elem->GetName() == "link") + { if (elem->HasElementDescription("pose")) { ignition::math::Pose3d offsetPose = @@ -1092,11 +1099,6 @@ void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF) } else if (elem->GetName() == "joint") { - // for joints, we need to - // prefix name like we did with links, and - std::string elemName = elem->Get("name"); - std::string newName = modelName + "::" + elemName; - replace[elemName] = newName; // rotate the joint axis because they are model-global if (elem->HasElement("axis")) { @@ -1113,12 +1115,18 @@ void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF) for (std::map::iterator iter = replace.begin(); iter != replace.end(); ++iter) { - replace_all(str, std::string("\"") + iter->first + "\"", - std::string("\"") + iter->second + "\""); - replace_all(str, std::string("'") + iter->first + "'", - std::string("'") + iter->second + "'"); - replace_all(str, std::string(">") + iter->first + "<", - std::string(">") + iter->second + "<"); + std::string oldName(iter->first); + std::string nameWithNestedPrefix(iter->second); + replace_all(str, std::string("\"") + oldName + "\"", + std::string("\"") + nameWithNestedPrefix + "\""); + replace_all(str, std::string("'") + oldName + "'", + std::string("'") + nameWithNestedPrefix + "'"); + replace_all(str, std::string(">") + oldName + "<", + std::string(">") + nameWithNestedPrefix + "<"); + // Deal with nested model inside other nested model and already with + // ::namespace:: entries in the name + replace_all(str, std::string(">") + oldName + "::", + std::string(">") + nameWithNestedPrefix + "::"); } _includeSDF->ClearElements(); diff --git a/test/integration/model/nested_names_test/model.config b/test/integration/model/nested_names_test/model.config new file mode 100644 index 000000000..6bb3d1f60 --- /dev/null +++ b/test/integration/model/nested_names_test/model.config @@ -0,0 +1,14 @@ + + + + Submodel + 0.1.0 + submodel_test.sdf + + Christina Gomez + cgomez@swri.org + + + A hinged door with two handles + + diff --git a/test/integration/model/nested_names_test/submodel_test.sdf b/test/integration/model/nested_names_test/submodel_test.sdf new file mode 100644 index 000000000..2bd5d5294 --- /dev/null +++ b/test/integration/model/nested_names_test/submodel_test.sdf @@ -0,0 +1,21 @@ + + + + + 0.06 -0.0005 0 0 -0 0 + + + + + + + subnested_model + subnested_model::link1 + + + + main_model_prefix::subnested_model::link1 + joint1 + + + diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 94ec7564d..48c2583c6 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -24,6 +24,8 @@ #include "sdf/sdf.hh" +#include "test_config.h" + //////////////////////////////////////// // Test parsing nested model with joint TEST(NestedModel, NestedModel) @@ -239,3 +241,51 @@ TEST(NestedModel, State) EXPECT_EQ(nestedLinkStateElem->Get("wrench"), ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); } + +//////////////////////////////////////// +// Test parsing a include element that has a pose element and includes a +// submodel +TEST(NestedModel, IncludeFlatteningNames) +{ + const std::string MODEL_PATH = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", + "model", "nested_names_test"); + + std::ostringstream stream; + std::string version = SDF_VERSION; + stream + << "" + << "" + << " " + << " " + MODEL_PATH +"" + << " " + << "" + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed)); + + sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model"); + EXPECT_EQ(modelElem->Get("name"), "top_level_model"); + + sdf::ElementPtr linkElem = modelElem->GetElement("link"); + EXPECT_EQ(linkElem->Get("name"), "main_model_prefix::frame"); + + sdf::ElementPtr jointElem = modelElem->GetElement("joint"); + EXPECT_EQ(jointElem->Get("name"), "main_model_prefix::joint1"); + EXPECT_EQ(jointElem->Get("parent"), + "main_model_prefix::subnested_model"); + EXPECT_EQ(jointElem->Get("child"), + "main_model_prefix::subnested_model::link1") << + "Flattening logic for nested models failed (check parser.cc)"; + + sdf::ElementPtr joint2Elem = jointElem->GetNextElement("joint"); + EXPECT_EQ(joint2Elem->Get("name"), "main_model_prefix::joint2"); + EXPECT_EQ(joint2Elem->Get("parent"), + "main_model_prefix::subnested_model::link1") << + "Flattening logic for nested models failed (check parser.cc)"; + EXPECT_EQ(joint2Elem->Get("child"), + "main_model_prefix::joint1") << + "Flattening logic for nested models failed (check parser.cc)"; +} From 41a2c3c165cbf84c1f4adc69586b4fc63fa42aa2 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Wed, 30 Jun 2021 20:48:03 +0200 Subject: [PATCH 08/60] Translate poses of nested models inside other nested models (sdf6) (#596) Translate poses of nested models inside other nested models When addNestedModel function is called, it processes links and joints to translate the pose accordingly to parent pose. This was not done for nested models inside the SDF being processed. The change includes nested models in the same way that is doing for links and add a test that fails without the change. Signed-off-by: Jose Luis Rivero Co-authored-by: Addisu Z. Taddese --- src/parser.cc | 2 +- test/integration/frame.cc | 60 +++++++++++++++++++ .../model/box_with_submodel/model.config | 15 +++++ .../model/box_with_submodel/model.sdf | 13 ++++ 4 files changed, 89 insertions(+), 1 deletion(-) create mode 100644 test/integration/model/box_with_submodel/model.config create mode 100644 test/integration/model/box_with_submodel/model.sdf diff --git a/src/parser.cc b/src/parser.cc index bfd1973ca..f101b1a53 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1084,7 +1084,7 @@ void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF) replace[elemName] = newName; } - if (elem->GetName() == "link") + if ((elem->GetName() == "link") || (elem->GetName() == "model")) { if (elem->HasElementDescription("pose")) { diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 6cd151b3f..f7485d6f8 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -1255,3 +1255,63 @@ TEST(Frame, IncludeFrame) EXPECT_EQ(modelPoseElem->Get(), ignition::math::Pose3d(5, -2, 1, 0, 0, 0)); } + +//////////////////////////////////////// +// Test parsing a include element that has a pose element and includes a +// submodel +TEST(Frame, IncludeFrameWithSubmodel) +{ + const std::string MODEL_PATH = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", + "model", "box_with_submodel"); + + std::ostringstream stream; + std::string version = SDF_VERSION; + stream + << "" + << "" + << "" + << " " + << " false" + << " 5 5 0 0 0 0" + << " " + MODEL_PATH +"" + << " " + << "" + << "" + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed)); + sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world"); + + /* top level model: using include will merge top_level_model and box_with_submodel into: + * + * + * 5 5 0 0 -0 0 <<-- pose has been translated + * + * + * ... + * + * + */ + sdf::ElementPtr modelElem = worldElem->GetElement("model"); + EXPECT_EQ(modelElem->Get("name"), "top_level_model"); + sdf::ElementPtr modelPoseElem = modelElem->GetElement("link")->GetElement("pose"); + EXPECT_EQ(modelPoseElem->Get(), + ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); + /* submodel: pose from parent is translated to model. links are the same + * ... + * + * + * 0 0 0 0 -0 0 + * + * 5 5 0 0 -0 0 + * + */ + sdf::ElementPtr subModelElem = modelElem->GetElement("model"); + EXPECT_EQ(subModelElem->Get("name"), "box_with_submodel::submodel_of_box_with_submodel"); + sdf::ElementPtr subModelPoseElem = subModelElem->GetElement("pose"); + EXPECT_EQ(subModelPoseElem->Get(), + ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); +} diff --git a/test/integration/model/box_with_submodel/model.config b/test/integration/model/box_with_submodel/model.config new file mode 100644 index 000000000..4db7fabb4 --- /dev/null +++ b/test/integration/model/box_with_submodel/model.config @@ -0,0 +1,15 @@ + + + BoxWithSubmodel + 1.0 + model.sdf + + + Jose Luis Rivero + jrivero@osrfoundation.org + + + + A box with a nested model + + diff --git a/test/integration/model/box_with_submodel/model.sdf b/test/integration/model/box_with_submodel/model.sdf new file mode 100644 index 000000000..be94c6dab --- /dev/null +++ b/test/integration/model/box_with_submodel/model.sdf @@ -0,0 +1,13 @@ + + + + + 0 0 0 0 0 0 + + + + 0 0 0 0 0 0 + + + + From 6deeb06d7c67eb7ecb08d37714003c657b2f6e17 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Wed, 7 Jul 2021 14:26:39 +0200 Subject: [PATCH 09/60] Prepare version 6.3.1 (#617) * Prepare version 6.3.1 * Implement github actions for sdf6 branch Signed-off-by: Steve Peters Signed-off-by: Jose Luis Rivero --- .github/ci/after_make_test.sh | 18 ++++++++++++++ .github/ci/packages.apt | 6 +++++ .github/workflows/ci.yml | 25 +++++++++++++++++++ .github/workflows/macos.yml | 45 +++++++++++++++++++++++++++++++++++ CMakeLists.txt | 2 +- Changelog.md | 10 +++++++- test/integration/frame.cc | 6 +++-- 7 files changed, 108 insertions(+), 4 deletions(-) create mode 100644 .github/ci/after_make_test.sh create mode 100644 .github/ci/packages.apt create mode 100644 .github/workflows/ci.yml create mode 100644 .github/workflows/macos.yml diff --git a/.github/ci/after_make_test.sh b/.github/ci/after_make_test.sh new file mode 100644 index 000000000..0aca36841 --- /dev/null +++ b/.github/ci/after_make_test.sh @@ -0,0 +1,18 @@ +#!/bin/sh -l + +set -x + +BUILD_DIR=`pwd` + +# Install +make install + +# Compile examples +cd ../examples +mkdir build; +cd build; +cmake ..; +make; +./simple ../simple.sdf; + +cd $BUILD_DIR diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt new file mode 100644 index 000000000..7cbfeb92a --- /dev/null +++ b/.github/ci/packages.apt @@ -0,0 +1,6 @@ +libignition-math4-dev +libignition-tools-dev +libboost-system-dev +libtinyxml-dev +python-psutil +ruby-dev diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 000000000..2914eb5d5 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,25 @@ +name: Ubuntu + +on: [push, pull_request] + +jobs: + bionic-ci: + runs-on: ubuntu-latest + name: Ubuntu Bionic CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@bionic + with: + codecov-enabled: true + focal-ci: + runs-on: ubuntu-latest + name: Ubuntu Focal CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@focal diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml new file mode 100644 index 000000000..870858218 --- /dev/null +++ b/.github/workflows/macos.yml @@ -0,0 +1,45 @@ +name: macOS latest + +on: [push, pull_request] + +jobs: + build: + + env: + PACKAGE: sdformat6 + runs-on: macos-latest + steps: + - uses: actions/checkout@v2 + - name: Set up Homebrew + id: set-up-homebrew + uses: Homebrew/actions/setup-homebrew@master + - run: brew config + + - name: Install base dependencies + run: | + brew tap osrf/simulation; + brew install --only-dependencies ${PACKAGE}; + + - run: mkdir build + - name: cmake + working-directory: build + run: cmake .. -DCMAKE_INSTALL_PREFIX=/usr/local/Cellar/${PACKAGE}/HEAD + - run: make + working-directory: build + - run: make test + working-directory: build + env: + CTEST_OUTPUT_ON_FAILURE: 1 + - name: make install + working-directory: build + run: | + make install; + brew link ${PACKAGE}; + - name: Compile example code + working-directory: examples + run: | + mkdir build; + cd build; + cmake ..; + make; + ./simple ../simple.sdf; diff --git a/CMakeLists.txt b/CMakeLists.txt index 911714956..3d401f1bc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ set (SDF_PROTOCOL_VERSION 1.6) set (SDF_MAJOR_VERSION 6) set (SDF_MINOR_VERSION 3) -set (SDF_PATCH_VERSION 0) +set (SDF_PATCH_VERSION 1) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}) diff --git a/Changelog.md b/Changelog.md index eae3784b8..47af0b3cd 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,14 @@ ## SDFormat 6.0 -### SDFormat 6.X.X (20XX-XX-XX) +### SDFormat 6.3.1 (2021-07-06) + +1. Fix flattening logic for nested model names + * [Pull request 597](https://github.com/osrf/sdformat/pull/597) + +1. Translate poses of nested models inside other nested models + * [Pull request 596](https://github.com/osrf/sdformat/pull/596) + +### SDFormat 6.3.0 (2021-06-21) 1. Move recursiveSameTypeUniqueNames from ign.cc to parser.cc and make public. * [Pull request 580](https://github.com/osrf/sdformat/pull/580) diff --git a/test/integration/frame.cc b/test/integration/frame.cc index f7485d6f8..0394b0638 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -1297,7 +1297,8 @@ TEST(Frame, IncludeFrameWithSubmodel) */ sdf::ElementPtr modelElem = worldElem->GetElement("model"); EXPECT_EQ(modelElem->Get("name"), "top_level_model"); - sdf::ElementPtr modelPoseElem = modelElem->GetElement("link")->GetElement("pose"); + sdf::ElementPtr modelPoseElem = + modelElem->GetElement("link")->GetElement("pose"); EXPECT_EQ(modelPoseElem->Get(), ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); /* submodel: pose from parent is translated to model. links are the same @@ -1310,7 +1311,8 @@ TEST(Frame, IncludeFrameWithSubmodel) * */ sdf::ElementPtr subModelElem = modelElem->GetElement("model"); - EXPECT_EQ(subModelElem->Get("name"), "box_with_submodel::submodel_of_box_with_submodel"); + EXPECT_EQ(subModelElem->Get("name"), + "box_with_submodel::submodel_of_box_with_submodel"); sdf::ElementPtr subModelPoseElem = subModelElem->GetElement("pose"); EXPECT_EQ(subModelPoseElem->Get(), ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); From feec202e7e358f2865ffb433db2147691554a195 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 13 Jul 2021 22:51:10 -0700 Subject: [PATCH 10/60] Use Ubuntu bionic in CI (#626) Signed-off-by: Steven Peters --- bitbucket-pipelines.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 1b04f6ee5..d363ade5c 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -1,4 +1,4 @@ -image: ubuntu:xenial +image: ubuntu:bionic pipelines: default: From f13264e855ddbf60c46f84333077b8766a5bc304 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 3 Aug 2021 17:34:16 -0700 Subject: [PATCH 11/60] Create CODEOWNERS with azeey and scpeters (#650) Signed-off-by: Steve Peters --- .github/CODEOWNERS | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 .github/CODEOWNERS diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 000000000..fa88665d8 --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1,4 @@ +# More info: +# https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners + +* @azeey @scpeters From b06cc1852bd16a609d0e86d592891885aed56ec3 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 23 Aug 2021 19:07:47 -0700 Subject: [PATCH 12/60] =?UTF-8?q?=F0=9F=91=A9=E2=80=8D=F0=9F=8C=BE=20Remov?= =?UTF-8?q?e=20bitbucket-pipelines=20and=20backport=20labeler=20/=20triage?= =?UTF-8?q?=20(#674)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- .github/workflows/pr-collection-labeler.yml | 13 ++++++++ .github/workflows/triage.yml | 19 ++++++++++++ bitbucket-pipelines.yml | 34 --------------------- 3 files changed, 32 insertions(+), 34 deletions(-) create mode 100644 .github/workflows/pr-collection-labeler.yml create mode 100644 .github/workflows/triage.yml delete mode 100644 bitbucket-pipelines.yml diff --git a/.github/workflows/pr-collection-labeler.yml b/.github/workflows/pr-collection-labeler.yml new file mode 100644 index 000000000..7d7b4e179 --- /dev/null +++ b/.github/workflows/pr-collection-labeler.yml @@ -0,0 +1,13 @@ +name: PR Collection Labeler + +on: pull_request_target + +jobs: + pr_collection_labeler: + runs-on: ubuntu-latest + steps: + - name: Add collection labels + if: github.event.action == 'opened' + uses: ignition-tooling/pr-collection-labeler@v1 + with: + github-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml new file mode 100644 index 000000000..736670e0e --- /dev/null +++ b/.github/workflows/triage.yml @@ -0,0 +1,19 @@ +on: + issues: + types: [opened] + pull_request_target: + types: [opened] +name: Ticket opened +jobs: + assign: + name: Add ticket to inbox + runs-on: ubuntu-latest + steps: + - name: Add ticket to inbox + uses: technote-space/create-project-card-action@v1 + with: + PROJECT: Core development + COLUMN: Inbox + GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }} + CHECK_ORG_PROJECT: true + diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml deleted file mode 100644 index d363ade5c..000000000 --- a/bitbucket-pipelines.yml +++ /dev/null @@ -1,34 +0,0 @@ -image: ubuntu:bionic - -pipelines: - default: - - step: - script: - # Dependencies - - apt update - - apt -y install cmake build-essential lcov curl git lsb-release wget - libtinyxml-dev libxml2-utils ruby-dev libboost-dev - python-psutil - - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-latest.list' - - wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - - - apt update - - apt install -y - libignition-math4-dev - libignition-tools-dev - # SDFormat - - mkdir build - - cd build - - cmake .. -DCMAKE_BUILD_TYPE=coverage - - make -j4 - - make test - - make coverage - - bash <(curl -s https://codecov.io/bash) - - make install - # Examples - - cd .. - - cd examples - - mkdir build - - cd build - - cmake .. - - make -j4 - - ./simple ../simple.sdf From c02aca7fc689592ba10e15baa1f6cea88e405186 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 30 Aug 2021 13:59:46 -0700 Subject: [PATCH 13/60] Fix xyz and rpy offsets in fixed joint reduction (#500) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * parse rpyOffset as radians Signed-off-by: Ian Chen * update tf for xyz and rpy offsets Signed-off-by: Ian Chen * remove inverse transform function in urdf parser Signed-off-by: Ian Chen * inject corrected_offets tag Signed-off-by: Ian Chen * Fix tag removal logic Signed-off-by: Steven Peters Co-authored-by: Alejandro Hernández Cordero Co-authored-by: Steve Peters --- src/parser_urdf.cc | 41 ++----- test/integration/fixed_joint_reduction.cc | 29 +++++ ...oint_reduction_plugin_frame_extension.urdf | 103 ++++++++++++++++++ 3 files changed, 143 insertions(+), 30 deletions(-) create mode 100644 test/integration/fixed_joint_reduction_plugin_frame_extension.urdf diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 39774457f..a83ee21c1 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -219,10 +219,6 @@ std::string Values2str(unsigned int _count, const double *_values); void CreateGeometry(TiXmlElement* _elem, urdf::GeometrySharedPtr _geometry); -ignition::math::Pose3d inverseTransformToParentFrame( - ignition::math::Pose3d _transformInLinkFrame, - urdf::Pose _parentToLinkTransform); - /// reduced fixed joints: transform to parent frame urdf::Pose TransformToParentFrame(urdf::Pose _transformInLinkFrame, urdf::Pose _parentToLinkTransform); @@ -2420,31 +2416,6 @@ ignition::math::Pose3d TransformToParentFrame( return transformInParentLinkFrame; } -///////////////////////////////////////////////// -/// reduced fixed joints: transform to parent frame -ignition::math::Pose3d inverseTransformToParentFrame( - ignition::math::Pose3d _transformInLinkFrame, - urdf::Pose _parentToLinkTransform) -{ - ignition::math::Pose3d transformInParentLinkFrame; - // rotate link pose to parentLink frame - urdf::Rotation ri = _parentToLinkTransform.rotation.GetInverse(); - ignition::math::Quaterniond q1(ri.w, ri.x, ri.y, ri.z); - transformInParentLinkFrame.Pos() = q1 * _transformInLinkFrame.Pos(); - urdf::Rotation r2 = _parentToLinkTransform.rotation.GetInverse(); - ignition::math::Quaterniond q3(r2.w, r2.x, r2.y, r2.z); - transformInParentLinkFrame.Rot() = q3 * _transformInLinkFrame.Rot(); - // translate link to parentLink frame - transformInParentLinkFrame.Pos().X() = transformInParentLinkFrame.Pos().X() - - _parentToLinkTransform.position.x; - transformInParentLinkFrame.Pos().Y() = transformInParentLinkFrame.Pos().Y() - - _parentToLinkTransform.position.y; - transformInParentLinkFrame.Pos().Z() = transformInParentLinkFrame.Pos().Z() - - _parentToLinkTransform.position.z; - - return transformInParentLinkFrame; -} - //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link) { @@ -3473,14 +3444,21 @@ void ReduceSDFExtensionPluginFrameReplace( // remove xyzOffset and rpyOffset (*_blobIt)->RemoveChild(rpyKey); } + TiXmlNode* correctedOffsetKey = + (*_blobIt)->FirstChild("ignition::corrected_offsets"); + if (correctedOffsetKey) + { + (*_blobIt)->RemoveChild(correctedOffsetKey); + } // pass through the parent transform from fixed joint reduction - _reductionTransform = inverseTransformToParentFrame(_reductionTransform, + _reductionTransform = TransformToParentFrame(_reductionTransform, _link->parent_joint->parent_to_joint_origin_transform); // create new offset xml blocks xyzKey = new TiXmlElement("xyzOffset"); rpyKey = new TiXmlElement("rpyOffset"); + correctedOffsetKey = new TiXmlElement("ignition::corrected_offsets"); // create new offset xml blocks urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), @@ -3501,12 +3479,15 @@ void ReduceSDFExtensionPluginFrameReplace( TiXmlText* xyzTxt = new TiXmlText(xyzStream.str()); TiXmlText* rpyTxt = new TiXmlText(rpyStream.str()); + TiXmlText* correctedOffsetTxt = new TiXmlText("1"); xyzKey->LinkEndChild(xyzTxt); rpyKey->LinkEndChild(rpyTxt); + correctedOffsetKey->LinkEndChild(correctedOffsetTxt); (*_blobIt)->LinkEndChild(xyzKey); (*_blobIt)->LinkEndChild(rpyKey); + (*_blobIt)->LinkEndChild(correctedOffsetKey); } } } diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index 6e2d5b628..f15080d3c 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -48,6 +48,9 @@ const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT = const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", "fixed_joint_reduction_collision_visual_empty_root.sdf"); +const std::string SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", + "fixed_joint_reduction_plugin_frame_extension.urdf"); const double gc_tolerance = 1e-6; @@ -736,3 +739,29 @@ TEST(SDFParser, FixedJointReductionSimple) EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance); } } + +///////////////////////////////////////////////// +// This test uses a urdf that has chained fixed joints with plugin that +// contains bodyName, xyzOffset and rpyOffset. +// Test to make sure the offsets have the correct transfrom and frame of +// reference +TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest) +{ + sdf::SDFPtr robot(new sdf::SDF()); + sdf::init(robot); + ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION, robot)); + + sdf::ElementPtr model = robot->Root()->GetElement("model"); + sdf::ElementPtr plugin = model->GetElement("plugin"); + + auto xyzOffset = plugin->Get("xyzOffset"); + auto rpyOffset = plugin->Get("rpyOffset"); + auto bodyName = plugin->Get("bodyName"); + EXPECT_EQ("base_link", bodyName); + EXPECT_EQ(ignition::math::Vector3d(-0.707108, 1.70711, 0), xyzOffset); + EXPECT_EQ(ignition::math::Vector3d(0, 0, 1.5708), rpyOffset); + + bool correctedOffset = plugin->Get("ignition::corrected_offsets"); + EXPECT_TRUE(correctedOffset); +} + diff --git a/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf b/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf new file mode 100644 index 000000000..4c2293670 --- /dev/null +++ b/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf @@ -0,0 +1,103 @@ + + + + + /test/plugin/service + /test/plugin/topic + link2 + 100 + 0 0 0 + 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 8cbc9b56d48f09db3e39dc4a784d33e3a5fde63c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 7 Sep 2021 13:35:13 -0700 Subject: [PATCH 14/60] Bump to 9.6.1 (#691) Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 20 ++++++++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 62985860f..393c91368 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set (SDF_PROTOCOL_VERSION 1.7) set (SDF_MAJOR_VERSION 9) set (SDF_MINOR_VERSION 6) -set (SDF_PATCH_VERSION 0) +set (SDF_PATCH_VERSION 1) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}) diff --git a/Changelog.md b/Changelog.md index cb37a087a..4c784e072 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,26 @@ ### libsdformat 9.X.X (202X-XX-XX) +### libsdformat 9.6.1 (2021-09-07) + +1. Parse URDF continuous joint effort/velocity limits + * [Pull request #684](https://github.com/ignitionrobotics/sdformat/pull/684) + +1. Add a codecheck make target + * [Pull request #682](https://github.com/ignitionrobotics/sdformat/pull/682) + +1. Refactor sdf::readXml + * [Pull request #681](https://github.com/ignitionrobotics/sdformat/pull/681) + +1. Upgrade cpplint and fix new errors + * [Pull request #680](https://github.com/ignitionrobotics/sdformat/pull/680) + +1. BUG: add missing plugin element to include + * [Pull request #675](https://github.com/ignitionrobotics/sdformat/pull/675) + +1. Added comment reminder to update functions + * [Pull request #677](https://github.com/ignitionrobotics/sdformat/pull/677) + ### libsdformat 9.6.0 (2021-08-18) 1. Adds `enable_metrics` flag to Sensor. From a55f9e3ea8ae5e9e2a5f5ad2919f68985dacf151 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 8 Sep 2021 18:04:31 -0700 Subject: [PATCH 15/60] =?UTF-8?q?=F0=9F=8E=88=2010.6.0=20(#694)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 73 +++++++++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 73 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c3c111e72..b8a53b9b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ project (sdformat10) set (SDF_PROTOCOL_VERSION 1.7) set (SDF_MAJOR_VERSION 10) -set (SDF_MINOR_VERSION 5) +set (SDF_MINOR_VERSION 6) set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) diff --git a/Changelog.md b/Changelog.md index d254f7882..3d5f8feee 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,76 @@ ## libsdformat 10.X +### libsdformat 10.6.0 (2021-09-08) + +1. Parse URDF continuous joint effort/velocity limits + * [Pull request #684](https://github.com/ignitionrobotics/sdformat/pull/684) + +1. Add enable_orientation SDF element to imu + * [Pull request #651](https://github.com/ignitionrobotics/sdformat/pull/651) + +1. Add a codecheck make target + * [Pull request #682](https://github.com/ignitionrobotics/sdformat/pull/682) + +1. Refactor sdf::readXml + * [Pull request #681](https://github.com/ignitionrobotics/sdformat/pull/681) + +1. Upgrade cpplint and fix new errors + * [Pull request #680](https://github.com/ignitionrobotics/sdformat/pull/680) + +1. BUG: add missing plugin element to include + * [Pull request #675](https://github.com/ignitionrobotics/sdformat/pull/675) + +1. Added comment reminder to update functions + * [Pull request #677](https://github.com/ignitionrobotics/sdformat/pull/677) + +1. Adds enable_metrics flag to Sensor. + * [Pull request #665](https://github.com/ignitionrobotics/sdformat/pull/665) + +1. Add GPS / NavSat sensor to sdf9 + * [Pull request #453](https://github.com/ignitionrobotics/sdformat/pull/453) + +1. Support parsing elements that are not part of the schema + * [Pull request #638](https://github.com/ignitionrobotics/sdformat/pull/638) + +1. Add lightmap to 1.7 spec and PBR material DOM + * [Pull request #429](https://github.com/ignitionrobotics/sdformat/pull/429) + +1. Fix urdf link extension tags + * [Pull request #628](https://github.com/ignitionrobotics/sdformat/pull/628) + +1. Updated material spec + * [Pull request #644](https://github.com/ignitionrobotics/sdformat/pull/644) + +1. Minor fix to Migration guide + * [Pull request #630](https://github.com/ignitionrobotics/sdformat/pull/630) + +1. Error: move << operator from .hh to .cc file + * [Pull request #625](https://github.com/ignitionrobotics/sdformat/pull/625) + +1. Update build system to allow overriding CXX flags and using clang on Linux + * [Pull request #621](https://github.com/ignitionrobotics/sdformat/pull/621) + +1. Add Element::FindElement as an alternative to Element::GetElement + * [Pull request #620](https://github.com/ignitionrobotics/sdformat/pull/620) + +1. Add ValidateGraphs methods to Model/World (sdf9) + * [Pull request #602](https://github.com/ignitionrobotics/sdformat/pull/602) + +1. Fix ABI break + * [Pull request #605](https://github.com/ignitionrobotics/sdformat/pull/605) + +1. Parameter passing prototype + * [Pull request #413](https://github.com/ignitionrobotics/sdformat/pull/413) + +1. Port particle scatter ratio param to sdf 1.6 + * [Pull request #595](https://github.com/ignitionrobotics/sdformat/pull/595) + +1. Making PrintValues() and ToString() able to not print default elements + * [Pull request #575](https://github.com/ignitionrobotics/sdformat/pull/575) + +1. Add API for determining if an element was set by the user + * [Pull request #542](https://github.com/ignitionrobotics/sdformat/pull/542) + ### libsdformat 10.5.0 (2021-05-17) 1. Add scatter ratio parameter to Particle Emitter DOM. @@ -143,7 +214,7 @@ 1. Adds `enable_metrics` flag to Sensor. * [Pull request #665](https://github.com/ignitionrobotics/sdformat/pull/665) -1. Add GPS sensor DOM to sdf9 +1. Add GPS / NavSat sensor DOM to sdf9 * [Pull request #453](https://github.com/ignitionrobotics/sdformat/pull/453) 1. Support parsing elements that are not part of the schema From c9494fbeaa36f45f085921f2b509a4074f3984ca Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 30 Sep 2021 17:33:35 -0500 Subject: [PATCH 16/60] =?UTF-8?q?=F0=9F=8E=88=2012.0.0=20(#724)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 49 ++++++++++++++++++++++++++++++++++--------------- README.md | 2 +- 3 files changed, 36 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 76052d478..a7370d855 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,7 +31,7 @@ set (SDF_MINOR_VERSION 0) set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) -set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}~pre2) +set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}) string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) string(REGEX REPLACE "[0-9]+" "" PROJECT_NAME_NO_VERSION ${PROJECT_NAME}) diff --git a/Changelog.md b/Changelog.md index 46fbe8c96..49da122bf 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,51 +1,69 @@ ## libsdformat 12.X -### libsdformat 12.0.0 (202X-XX-XX) +### libsdformat 12.0.0 (2021-09-30) -1. Add API changes for PrintConfig +1. Make exception for plugins when checking for name uniqueness + * [Pull request #721](https://github.com/ignitionrobotics/sdformat/pull/721) + +1. Remove empty //inertial/pose/@relative_to during 1_7->1.8 conversion + * [Pull request #720](https://github.com/ignitionrobotics/sdformat/pull/720) + +1. Added macos install instructions to README.md + * [Pull request #714](https://github.com/ignitionrobotics/sdformat/pull/714) + +1. Do not automatically remove //axis/initial_position + * [Pull request #717](https://github.com/ignitionrobotics/sdformat/pull/717) + +1. DOC: don't mention elements that can't be included + * [Pull request #715](https://github.com/ignitionrobotics/sdformat/pull/715) + +1. Prefix merged frames with an underscore + * [Pull request #711](https://github.com/ignitionrobotics/sdformat/pull/711) + +1. Add API changes for PrintConfig * [Pull request #708](https://github.com/ignitionrobotics/sdformat/pull/708) -1. Add Force Torque Noise functions + Unit tests +1. Add Force Torque Noise functions + Unit tests * [Pull request #669](https://github.com/ignitionrobotics/sdformat/pull/669) -1. Support quaternions representation for poses +1. Support quaternions representation for poses * [Pull request #690](https://github.com/ignitionrobotics/sdformat/pull/690) -1. Support merge-include of nested models +1. Support merge-include of nested models * [Pull request #659](https://github.com/ignitionrobotics/sdformat/pull/659) -1. Fix documentation on Euler angle convention in pose.sdf +1. Fix documentation on Euler angle convention in pose.sdf * [Pull request #698](https://github.com/ignitionrobotics/sdformat/pull/698) * [Pull request #702](https://github.com/ignitionrobotics/sdformat/pull/702) -1. Clarify documentation on //pose/@relative_to in the spec +1. Clarify documentation on //pose/@relative_to in the spec * [Pull request #666](https://github.com/ignitionrobotics/sdformat/pull/666) -1. 🌐 Parse spherical coordinates +1. 🌐 Parse spherical coordinates * [Pull request #685](https://github.com/ignitionrobotics/sdformat/pull/685) 1. Fix bug when using degrees in //include/pose * [Pull request #697](https://github.com/ignitionrobotics/sdformat/pull/697) -1. Support rotation in degrees (#589) +1. Support rotation in degrees (#589) * [Pull request #589](https://github.com/ignitionrobotics/sdformat/pull/589) -1. Add segmentation and bounding box sensor types +1. Add segmentation and bounding box sensor types * [Pull request #592](https://github.com/ignitionrobotics/sdformat/pull/592) -1. Add support for custom sensors +1. Add support for custom sensors * [Pull request #652](https://github.com/ignitionrobotics/sdformat/pull/652) -1. Remove deprecated functions and classes +1. Remove deprecated functions and classes * [Pull request #622](https://github.com/ignitionrobotics/sdformat/pull/622) -1. Emit an error instead of a warning when a file has multiple root level +1. Emit an error instead of a warning when a file has multiple root level * [Pull request #619](https://github.com/ignitionrobotics/sdformat/pull/619) -1. Copy spec 1.8 to 1.9 +1. Copy spec 1.8 to 1.9 * [Pull request #568](https://github.com/ignitionrobotics/sdformat/pull/568) -1. Use encapsulated string constants for non-file sources +1. Use encapsulated string constants for non-file sources * [Pull request #551](https://github.com/ignitionrobotics/sdformat/pull/551) 1. Forward ports @@ -57,6 +75,7 @@ * [Pull request #660](https://github.com/ignitionrobotics/sdformat/pull/660) * [Pull request #693](https://github.com/ignitionrobotics/sdformat/pull/693) * [Pull request #706](https://github.com/ignitionrobotics/sdformat/pull/706) + * [Pull request #723](https://github.com/ignitionrobotics/sdformat/pull/723) 1. Infrastructure * [Pull request #532](https://github.com/ignitionrobotics/sdformat/pull/532) diff --git a/README.md b/README.md index f483a3397..392e0c525 100644 --- a/README.md +++ b/README.md @@ -32,7 +32,7 @@ TODO(eric.cousineau): Move terminology section to sdf_tutorials? ## Test coverage -[![codecov](https://codecov.io/gh/ignitionrobotics/sdformat/branch/main/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/sdformat/branch/main) +[![codecov](https://codecov.io/gh/ignitionrobotics/sdformat/branch/sdf12/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/sdformat/branch/sdf12) # Installation From b0f7f4fcd38f2fd921a5995a5f91283b3d6d25a1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 14 Oct 2021 10:03:26 -0700 Subject: [PATCH 17/60] Codecheck fixes (#730) * Pass by const reference * Remove unused variable * Simplify redundant logic Signed-off-by: Steve Peters --- src/Converter.cc | 2 +- src/FrameSemantics.cc | 3 +-- src/parser.cc | 28 ++++++++-------------------- src/parser_TEST.cc | 2 +- test/integration/interface_api.cc | 2 +- test/integration/nested_model.cc | 2 +- test/integration/unknown.cc | 2 +- 7 files changed, 14 insertions(+), 27 deletions(-) diff --git a/src/Converter.cc b/src/Converter.cc index 542c14813..451f4b696 100644 --- a/src/Converter.cc +++ b/src/Converter.cc @@ -88,7 +88,7 @@ bool Converter::Convert(tinyxml2::XMLDocument *_doc, return false; } - if (!elem || !elem->Attribute("version")) + if (!elem->Attribute("version")) { sdferr << " Unable to determine original SDF version\n"; return false; diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 94893dcf5..6081aac97 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -44,7 +44,6 @@ inline namespace SDF_VERSION_NAMESPACE { // Helpful functions when debugging in gdb void printGraph(const ScopedGraph &_graph) { - std::stringstream ss; std::cout << _graph.Graph() << std::endl; } void printGraph(const ScopedGraph &_graph) @@ -258,7 +257,7 @@ std::pair /// be modified. static Errors resolveModelPoseWithPlacementFrame( const ignition::math::Pose3d &_rawPose, - const std::string _placementFrame, + const std::string &_placementFrame, const sdf::ScopedGraph &_graph, ignition::math::Pose3d &_resolvedPose) { diff --git a/src/parser.cc b/src/parser.cc index 0b11d0f03..699c15fab 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -661,6 +661,7 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf, tinyxml2::XMLElement *sdfNode = _xmlDoc->FirstChildElement("sdf"); if (!sdfNode) { + sdfdbg << "No element in file[" << _source << "]\n"; return false; } @@ -675,7 +676,7 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf, _sdf->SetFilePath(_source); } - if (sdfNode && sdfNode->Attribute("version")) + if (sdfNode->Attribute("version")) { if (_sdf->OriginalVersion().empty()) { @@ -737,15 +738,8 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf, } else { - if (!sdfNode) - { - sdfdbg << "No element in file[" << _source << "]\n"; - } - else if (!sdfNode->Attribute("version")) - { - sdfdbg << "SDF element has no version in file[" - << _source << "]\n"; - } + sdfdbg << "SDF element has no version in file[" + << _source << "]\n"; return false; } @@ -767,6 +761,7 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf, tinyxml2::XMLElement *sdfNode = _xmlDoc->FirstChildElement("sdf"); if (!sdfNode) { + sdfdbg << "SDF has no element\n"; return false; } @@ -775,7 +770,7 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf, _sdf->SetFilePath(_source); } - if (sdfNode && sdfNode->Attribute("version")) + if (sdfNode->Attribute("version")) { if (_sdf->OriginalVersion().empty()) { @@ -838,14 +833,7 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf, } else { - if (!sdfNode) - { - sdfdbg << "SDF has no element\n"; - } - else if (!sdfNode->Attribute("version")) - { - sdfdbg << " element has no version\n"; - } + sdfdbg << " element has no version\n"; return false; } @@ -1149,7 +1137,7 @@ static bool readAttributes(tinyxml2::XMLElement *_xml, ElementPtr _sdf, /// \param[out] _errors Captures errors found during parsing. /// \return True if the file name is successfully resolved, false on error. static bool resolveFileNameFromUri(tinyxml2::XMLElement *_includeXml, - const sdf::ParserConfig &_config, const std::string _includeXmlPath, + const sdf::ParserConfig &_config, const std::string &_includeXmlPath, const std::string &_errorSourcePath, std::string &_fileName, Errors &_errors) { diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index 29beaee98..017c9cd9e 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -581,7 +581,7 @@ TEST_F(ValueConstraintsFixture, ElementMinMaxValues) auto sdf = InitSDF(); sdf->Root()->AddElementDescription(sdfTest->Root()); - auto wrapInSdf = [](std::string _xml) -> std::string + auto wrapInSdf = [](const std::string &_xml) -> std::string { std::stringstream ss; ss << "" << _xml diff --git a/test/integration/interface_api.cc b/test/integration/interface_api.cc index 6d7bdde83..77180114c 100644 --- a/test/integration/interface_api.cc +++ b/test/integration/interface_api.cc @@ -392,7 +392,7 @@ TEST_F(InterfaceAPI, FrameSemantics) EXPECT_EQ(1u, world->InterfaceModelCount()); auto resolvePoseNoErrors = - [](const sdf::SemanticPose &_semPose, const std::string _relativeTo = "") + [](const sdf::SemanticPose &_semPose, const std::string &_relativeTo = "") { Pose3d pose; sdf::Errors resolveErrors = _semPose.Resolve(pose, _relativeTo); diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 904115114..d6056274d 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -993,7 +993,7 @@ class PlacementFrame: public ::testing::Test } public: sdf::SemanticPose GetChildEntitySemanticPose( - const sdf::World *_world, const std::string _entityName) + const sdf::World *_world, const std::string &_entityName) { return _world->ModelByName(_entityName)->SemanticPose(); } diff --git a/test/integration/unknown.cc b/test/integration/unknown.cc index 35ade31b6..7e9b2ed5a 100644 --- a/test/integration/unknown.cc +++ b/test/integration/unknown.cc @@ -142,7 +142,7 @@ TEST(UnrecognizedElements, OldElementsInNewSchemas) { // This should be valid in 1.6, but not in 1.8 (do to usage of // `use_parent_model_frame`). - auto make_model_xml_string = [](std::string version) + auto make_model_xml_string = [](const std::string &version) { return R"( From b25ecbc75f6d54b0f35ceafb0222c8dd544fdbbc Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 15 Oct 2021 13:54:22 -0700 Subject: [PATCH 18/60] Check joint parent/child names in Root::Load (#727) * ign_TEST: expect joint_axis_infinite_limits valid * Add missing link6 to test/sdf/joint_axis_*.sdf Signed-off-by: Steve Peters --- include/sdf/parser.hh | 10 ++ src/Joint.cc | 2 +- src/Root.cc | 4 + src/ign_TEST.cc | 13 ++- src/parser.cc | 116 ++++++++++------------ test/integration/joint_dom.cc | 8 +- test/sdf/joint_axis_infinite_limits.sdf | 1 + test/sdf/joint_axis_xyz_normalization.sdf | 1 + 8 files changed, 84 insertions(+), 71 deletions(-) diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index 920e6d3c2..d5f9f5165 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -388,6 +388,16 @@ namespace sdf SDFORMAT_VISIBLE bool checkJointParentChildLinkNames(const sdf::Root *_root); + /// \brief Check that all joints in contained models specify parent + /// and child names that match the names of sibling links, joints, + /// models, or 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 checkJointParentChildNames(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/src/Joint.cc b/src/Joint.cc index c7847bf5b..2f30928bd 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -159,7 +159,7 @@ Errors Joint::Load(ElementPtr _sdf) { errors.push_back({ErrorCode::JOINT_PARENT_SAME_AS_CHILD, "Joint with name[" + this->dataPtr->name + - "] must specify different link names for " + "] must specify different frame names for " "parent and child, while [" + this->dataPtr->childLinkName + "] was specified for both."}); } diff --git a/src/Root.cc b/src/Root.cc index 8e9e205fd..5702efe71 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -325,6 +325,10 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) } } + // Check that Joint parent and child names resolve to valid and + // different frames. + checkJointParentChildNames(this, errors); + return errors; } diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index c3e8adfac..c5c120ee0 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -257,7 +257,7 @@ TEST(check, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) std::string output = custom_exec_str(IgnCommand() + " sdf -k " + path + SdfVersion()); EXPECT_NE(output.find("Joint with name[joint] must " - "specify different link names for parent and child, " + "specify different frame names for parent and child, " "while [link] was specified for both."), std::string::npos) << output; } @@ -322,6 +322,17 @@ TEST(check, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) EXPECT_EQ("Valid.\n", output) << output; } + // Check an SDF file with the infinite values for joint axis limits. + // This is a valid file. + { + std::string path = pathBase +"/joint_axis_infinite_limits.sdf"; + + // Check joint_axis_infinite_limits.sdf + std::string output = + custom_exec_str(IgnCommand() + " sdf -k " + path + SdfVersion()); + EXPECT_EQ("Valid.\n", output) << output; + } + // Check an SDF file with the second link specified as the canonical link. // This is a valid file. { diff --git a/src/parser.cc b/src/parser.cc index 699c15fab..e3feab8ae 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -2232,12 +2232,24 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) ////////////////////////////////////////////////// bool checkJointParentChildLinkNames(const sdf::Root *_root) { - bool result = true; + Errors errors; + checkJointParentChildNames(_root, errors); + if (!errors.empty()) + { + std::cerr << "Error when attempting to resolve child link name:" + << std::endl + << errors; + return false; + } + return true; +} +////////////////////////////////////////////////// +void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) +{ auto checkModelJointParentChildNames = []( - const sdf::Model *_model) -> bool + const sdf::Model *_model, Errors &errors) -> void { - bool modelResult = true; for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); @@ -2247,23 +2259,18 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) !_model->JointNameExists(parentName) && !_model->FrameNameExists(parentName)) { - std::cerr << "Error: parent frame with name[" << parentName - << "] specified by joint with name[" << joint->Name() - << "] not found in model with name[" << _model->Name() - << "]." - << std::endl; - modelResult = false; + 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() + "]."}); } const std::string &childName = joint->ChildLinkName(); if (childName == "world") { - std::cerr << "Error: invalid child name[world" - << "] specified by joint with name[" << joint->Name() - << "] in model with name[" << _model->Name() - << "]." - << std::endl; - modelResult = false; + 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 (!_model->LinkNameExists(childName) && @@ -2271,75 +2278,54 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) !_model->FrameNameExists(childName) && !_model->ModelNameExists(childName)) { - std::cerr << "Error: child frame with name[" << childName - << "] specified by joint with name[" << joint->Name() - << "] not found in model with name[" << _model->Name() - << "]." - << std::endl; - modelResult = false; + 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 (childName == joint->Name()) { - std::cerr << "Error: joint with name[" << joint->Name() - << "] in model with name[" << _model->Name() - << "] must not specify its own name as the child frame." - << std::endl; - modelResult = false; + 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."}); } if (parentName == joint->Name()) { - std::cerr << "Error: joint with name[" << joint->Name() - << "] in model with name[" << _model->Name() - << "] must not specify its own name as the parent frame." - << std::endl; - modelResult = false; + 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."}); } // Check that parent and child frames resolve to different links std::string resolvedChildName; std::string resolvedParentName; - auto errors = joint->ResolveChildLink(resolvedChildName); - if (!errors.empty()) - { - std::cerr << "Error when attempting to resolve child link name:" - << std::endl; - for (auto error : errors) - { - std::cerr << error.Message() << std::endl; - } - modelResult = false; - } - errors = joint->ResolveParentLink(resolvedParentName); - if (!errors.empty()) - { - std::cerr << "Error when attempting to resolve parent link name:" - << std::endl; - for (auto error : errors) - { - std::cerr << error.Message() << std::endl; - } - modelResult = false; - } + + 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()); + if (resolvedChildName == resolvedParentName) { - std::cerr << "Error: 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." - << std::endl; - modelResult = false; + 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."}); } } - return modelResult; }; if (_root->Model()) { - result = checkModelJointParentChildNames(_root->Model()) && result; + checkModelJointParentChildNames(_root->Model(), _errors); } for (uint64_t w = 0; w < _root->WorldCount(); ++w) @@ -2348,11 +2334,9 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) for (uint64_t m = 0; m < world->ModelCount(); ++m) { auto model = world->ModelByIndex(m); - result = checkModelJointParentChildNames(model) && result; + checkModelJointParentChildNames(model, _errors); } } - - return result; } ////////////////////////////////////////////////// diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index dc06205af..f579be94a 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -218,7 +218,7 @@ TEST(DOMJoint, LoadInvalidJointChildWorld) auto errors = root.Load(testFile); for (auto e : errors) std::cout << e << std::endl; - ASSERT_EQ(7u, errors.size()); + ASSERT_EQ(10u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -549,7 +549,7 @@ TEST(DOMJoint, LoadInvalidChild) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(6u, errors.size()); + EXPECT_EQ(8u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -564,6 +564,8 @@ TEST(DOMJoint, LoadInvalidChild) // errors[3] // errors[4] // errors[5] + // errors[6] + // errors[7] } ///////////////////////////////////////////////// @@ -584,7 +586,7 @@ TEST(DOMJoint, LoadLinkJointSameName17Invalid) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(7u, errors.size()); + EXPECT_EQ(9u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, errors[0].Message().find( diff --git a/test/sdf/joint_axis_infinite_limits.sdf b/test/sdf/joint_axis_infinite_limits.sdf index fb1ab831c..fd3c30b06 100644 --- a/test/sdf/joint_axis_infinite_limits.sdf +++ b/test/sdf/joint_axis_infinite_limits.sdf @@ -6,6 +6,7 @@ + link1 diff --git a/test/sdf/joint_axis_xyz_normalization.sdf b/test/sdf/joint_axis_xyz_normalization.sdf index 86b4ab34b..7b7efd317 100644 --- a/test/sdf/joint_axis_xyz_normalization.sdf +++ b/test/sdf/joint_axis_xyz_normalization.sdf @@ -6,6 +6,7 @@ + link1 From aa9362b47a63189cf467d6810ec2408b4fb764e3 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 15 Oct 2021 13:55:09 -0700 Subject: [PATCH 19/60] Check joint parent link names in Model::Load (#726) * ign_TEST: expect joint_axis_infinite_limits valid * Add missing link6 to test/sdf/joint_axis_*.sdf Signed-off-by: Steve Peters --- src/Model.cc | 9 +++++++++ src/ign_TEST.cc | 11 +++++++++++ test/sdf/joint_axis_infinite_limits.sdf | 1 + test/sdf/joint_axis_xyz_normalization.sdf | 1 + 4 files changed, 22 insertions(+) diff --git a/src/Model.cc b/src/Model.cc index a2c2aa947..96be0312e 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -397,9 +397,18 @@ Errors Model::Load(ElementPtr _sdf) errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), setPoseRelativeToGraphErrors.end()); } + // Pass graph pointer to joint and check parent names for (auto &joint : this->dataPtr->joints) { joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + const std::string &parentLinkName = joint.ParentLinkName(); + if (parentLinkName != "world" && !this->LinkByName(parentLinkName)) + { + errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, + "parent link with name[" + parentLinkName + + "] specified by joint with name[" + joint.Name() + + "] not found in model with name[" + this->Name() + "]."}); + } } for (auto &frame : this->dataPtr->frames) { diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 2d9f86e3c..44e5dbee5 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -260,6 +260,17 @@ TEST(check, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) EXPECT_EQ("Valid.\n", output) << output; } + // Check an SDF file with the infinite values for joint axis limits. + // This is a valid file. + { + std::string path = pathBase +"/joint_axis_infinite_limits.sdf"; + + // Check joint_axis_infinite_limits.sdf + std::string output = + custom_exec_str(IgnCommand() + " sdf -k " + path + SdfVersion()); + EXPECT_EQ("Valid.\n", output) << output; + } + // Check an SDF file with the second link specified as the canonical link. // This is a valid file. { diff --git a/test/sdf/joint_axis_infinite_limits.sdf b/test/sdf/joint_axis_infinite_limits.sdf index fb1ab831c..fd3c30b06 100644 --- a/test/sdf/joint_axis_infinite_limits.sdf +++ b/test/sdf/joint_axis_infinite_limits.sdf @@ -6,6 +6,7 @@ + link1 diff --git a/test/sdf/joint_axis_xyz_normalization.sdf b/test/sdf/joint_axis_xyz_normalization.sdf index 86b4ab34b..7b7efd317 100644 --- a/test/sdf/joint_axis_xyz_normalization.sdf +++ b/test/sdf/joint_axis_xyz_normalization.sdf @@ -6,6 +6,7 @@ + link1 From 344312ca05810f6c8010e4a0249a885305c915ef Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 18 Oct 2021 11:25:50 -0500 Subject: [PATCH 20/60] Backport test utilities from sdf10 (#731) Adds sdf::testing::SourceFile, sdf::testing::TestFile, sdf::testing::env, and sdf::testing::setenv and updates tests to use them. This makes it easier to backport other changes from newer branches. Signed-off-by: Addisu Z. Taddese --- src/CMakeLists.txt | 1 + src/Converter_TEST.cc | 6 +- src/FrameSemantics_TEST.cc | 9 +- src/ign_TEST.cc | 8 +- src/parser_TEST.cc | 40 ++--- test/env.hh | 139 ++++++++++++++++++ test/integration/actor_dom.cc | 6 +- test/integration/audio.cc | 9 +- .../cfm_damping_implicit_spring_damper.cc | 10 +- test/integration/collision_dom.cc | 6 +- test/integration/converter.cc | 7 +- .../disable_fixed_joint_reduction.cc | 9 +- test/integration/element_memory_leak.cc | 2 +- test/integration/fixed_joint_reduction.cc | 15 +- test/integration/force_torque_sensor.cc | 4 +- test/integration/frame.cc | 32 ++-- test/integration/geometry_dom.cc | 5 +- test/integration/include.cc | 3 +- test/integration/includes.cc | 23 +-- test/integration/joint_axis_dom.cc | 6 +- test/integration/joint_dom.cc | 28 ++-- test/integration/light_dom.cc | 3 +- test/integration/link_dom.cc | 22 +-- test/integration/material_pbr.cc | 6 +- test/integration/model_dom.cc | 20 +-- test/integration/model_versions.cc | 16 +- test/integration/plugin_include.cc | 8 +- test/integration/provide_feedback.cc | 4 +- test/integration/root_dom.cc | 15 +- test/integration/scene_dom.cc | 3 +- test/integration/sdf_custom.cc | 4 +- test/integration/surface_dom.cc | 4 +- test/integration/urdf_gazebo_extensions.cc | 3 +- test/integration/urdf_joint_parameters.cc | 4 +- test/integration/visual_dom.cc | 21 +-- test/integration/world_dom.cc | 16 +- test/performance/parser_urdf.cc | 7 +- test/test_config.h.in | 115 ++++++++++++--- 38 files changed, 390 insertions(+), 249 deletions(-) create mode 100644 test/env.hh diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8329a5cf3..08362bf88 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -151,6 +151,7 @@ if (BUILD_SDF_TEST) ) endif() + include_directories(${PROJECT_SOURCE_DIR}/test) sdf_build_tests(${gtest_sources}) if (NOT WIN32) diff --git a/src/Converter_TEST.cc b/src/Converter_TEST.cc index 0bbbbf091..49ddec986 100644 --- a/src/Converter_TEST.cc +++ b/src/Converter_TEST.cc @@ -1887,13 +1887,11 @@ TEST(Converter, MuchNewerVersion) static std::string ConvertDoc_15_16() { - return sdf::filesystem::append(PROJECT_SOURCE_PATH, "sdf", "1.6", - "1_5.convert"); + return sdf::testing::SourceFile("sdf", "1.6", "1_5.convert"); } static std::string ConvertDoc_16_17() { - return sdf::filesystem::append(PROJECT_SOURCE_PATH, "sdf", "1.7", - "1_6.convert"); + return sdf::testing::SourceFile("sdf", "1.7", "1_6.convert"); } ///////////////////////////////////////////////// diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index e119b589b..27fad211e 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -38,8 +38,7 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_frame_attached_to.sdf"); + sdf::testing::TestFile("sdf", "model_frame_attached_to.sdf"); // Load the SDF file sdf::Root root; @@ -103,8 +102,7 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model) TEST(FrameSemantics, buildFrameAttachedToGraph_World) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "world_frame_attached_to.sdf"); + sdf::testing::TestFile("sdf", "world_frame_attached_to.sdf"); // Load the SDF file sdf::Root root; @@ -198,8 +196,7 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) TEST(FrameSemantics, buildPoseRelativeToGraph) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_frame_relative_to_joint.sdf"); + sdf::testing::TestFile("sdf", "model_frame_relative_to_joint.sdf"); // Load the SDF file sdf::Root root; diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index fbdc34582..73935b64a 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -721,7 +721,7 @@ int main(int argc, char **argv) { // Set IGN_CONFIG_PATH to the directory where the .yaml configuration file // is located. - setenv("IGN_CONFIG_PATH", IGN_CONFIG_PATH, 1); + sdf::testing::setenv("IGN_CONFIG_PATH", IGN_CONFIG_PATH); // Make sure that we load the library recently built and not the one installed // in your system. This is done by placing the the current build directory @@ -732,13 +732,13 @@ int main(int argc, char **argv) #ifndef _WIN32 std::string testLibraryPath = IGN_TEST_LIBRARY_PATH; - char *currentLibraryPath = std::getenv("LD_LIBRARY_PATH"); - if (currentLibraryPath) + std::string currentLibraryPath; + if (sdf::testing::env("LD_LIBRARY_PATH", currentLibraryPath)) { testLibraryPath = testLibraryPath + ":" + currentLibraryPath; } - setenv("LD_LIBRARY_PATH", testLibraryPath.c_str(), 1); + sdf::testing::setenv("LD_LIBRARY_PATH", testLibraryPath); #endif ::testing::InitGoogleTest(&argc, argv); diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index bd434c1fd..9d6f05b3c 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -62,10 +62,9 @@ sdf::SDFPtr InitSDF() ///////////////////////////////////////////////// TEST(Parser, ReusedSDFVersion) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - const std::string path17 = pathBase +"/model_link_relative_to.sdf"; - const std::string path16 = pathBase +"/joint_complete.sdf"; + const auto path17 = sdf::testing::TestFile( + "sdf", "model_link_relative_to.sdf"); + const auto path16 = sdf::testing::TestFile("sdf", "joint_complete.sdf"); // Call readFile API that always converts sdf::SDFPtr sdf = InitSDF(); @@ -85,9 +84,7 @@ TEST(Parser, ReusedSDFVersion) ///////////////////////////////////////////////// TEST(Parser, readFileConversions) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - const std::string path = pathBase +"/joint_complete.sdf"; + const auto path = sdf::testing::TestFile("sdf", "joint_complete.sdf"); // Call readFile API that always converts { @@ -346,16 +343,13 @@ TEST(Parser, addNestedModel) ///////////////////////////////////////////////// TEST(Parser, NameUniqueness) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - // These tests are copies of the ones in ign_TEST.cc but use direct calls to // name uniqueness validator functions instead of going through ign. // Check an SDF file with sibling elements of the same type (world) // that have duplicate names. { - std::string path = pathBase +"/world_duplicate.sdf"; + const auto path = sdf::testing::TestFile("sdf", "world_duplicate.sdf"); sdf::SDFPtr sdf = InitSDF(); EXPECT_TRUE(sdf::readFile(path, sdf)); EXPECT_FALSE(sdf::recursiveSameTypeUniqueNames(sdf->Root())); @@ -368,7 +362,8 @@ TEST(Parser, NameUniqueness) // Check an SDF file with sibling elements of different types (model, light) // that have duplicate names. { - std::string path = pathBase +"/world_sibling_same_names.sdf"; + const auto path = sdf::testing::TestFile("sdf", + "world_sibling_same_names.sdf"); sdf::SDFPtr sdf = InitSDF(); EXPECT_TRUE(sdf::readFile(path, sdf)); EXPECT_FALSE(sdf::recursiveSiblingUniqueNames(sdf->Root())); @@ -381,7 +376,8 @@ TEST(Parser, NameUniqueness) // Check an SDF file with sibling elements of the same type (link) // that have duplicate names. { - std::string path = pathBase +"/model_duplicate_links.sdf"; + const auto path = sdf::testing::TestFile("sdf", + "model_duplicate_links.sdf"); sdf::SDFPtr sdf = InitSDF(); EXPECT_TRUE(sdf::readFile(path, sdf)); EXPECT_FALSE(sdf::recursiveSameTypeUniqueNames(sdf->Root())); @@ -394,7 +390,8 @@ TEST(Parser, NameUniqueness) // Check an SDF file with sibling elements of the same type (joint) // that have duplicate names. { - std::string path = pathBase +"/model_duplicate_joints.sdf"; + const auto path = sdf::testing::TestFile("sdf", + "model_duplicate_joints.sdf"); sdf::SDFPtr sdf = InitSDF(); EXPECT_TRUE(sdf::readFile(path, sdf)); EXPECT_FALSE(sdf::recursiveSameTypeUniqueNames(sdf->Root())); @@ -407,7 +404,8 @@ TEST(Parser, NameUniqueness) // Check an SDF file with sibling elements of different types (link, joint) // that have duplicate names. { - std::string path = pathBase +"/model_link_joint_same_name.sdf"; + const auto path = sdf::testing::TestFile("sdf", + "model_link_joint_same_name.sdf"); sdf::SDFPtr sdf = InitSDF(); EXPECT_TRUE(sdf::readFile(path, sdf)); EXPECT_FALSE(sdf::recursiveSiblingUniqueNames(sdf->Root())); @@ -420,7 +418,8 @@ TEST(Parser, NameUniqueness) // Check an SDF file with sibling elements of the same type (collision) // that have duplicate names. { - std::string path = pathBase +"/link_duplicate_sibling_collisions.sdf"; + const auto path = sdf::testing::TestFile("sdf", + "link_duplicate_sibling_collisions.sdf"); sdf::SDFPtr sdf = InitSDF(); EXPECT_TRUE(sdf::readFile(path, sdf)); EXPECT_FALSE(sdf::recursiveSameTypeUniqueNames(sdf->Root())); @@ -433,7 +432,8 @@ TEST(Parser, NameUniqueness) // Check an SDF file with sibling elements of the same type (visual) // that have duplicate names. { - std::string path = pathBase +"/link_duplicate_sibling_visuals.sdf"; + const auto path = sdf::testing::TestFile("sdf", + "link_duplicate_sibling_visuals.sdf"); sdf::SDFPtr sdf = InitSDF(); EXPECT_TRUE(sdf::readFile(path, sdf)); EXPECT_FALSE(sdf::recursiveSiblingUniqueNames(sdf->Root())); @@ -446,7 +446,8 @@ TEST(Parser, NameUniqueness) // Check an SDF file with cousin elements of the same type (collision) // that have duplicate names. This is a valid file. { - std::string path = pathBase +"/link_duplicate_cousin_collisions.sdf"; + const auto path = sdf::testing::TestFile("sdf", + "link_duplicate_cousin_collisions.sdf"); sdf::SDFPtr sdf = InitSDF(); EXPECT_TRUE(sdf::readFile(path, sdf)); EXPECT_TRUE(sdf::recursiveSameTypeUniqueNames(sdf->Root())); @@ -460,7 +461,8 @@ TEST(Parser, NameUniqueness) // Check an SDF file with cousin elements of the same type (visual) // that have duplicate names. This is a valid file. { - std::string path = pathBase +"/link_duplicate_cousin_visuals.sdf"; + const auto path = sdf::testing::TestFile("sdf", + "link_duplicate_cousin_visuals.sdf"); sdf::SDFPtr sdf = InitSDF(); EXPECT_TRUE(sdf::readFile(path, sdf)); EXPECT_TRUE(sdf::recursiveSameTypeUniqueNames(sdf->Root())); diff --git a/test/env.hh b/test/env.hh new file mode 100644 index 000000000..660569429 --- /dev/null +++ b/test/env.hh @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +// Note: this is a direct copy from ignition::common, +// and can be removed when sdformat has common as a dependency + +#ifndef SDF_TEST_ENV_HH +#define SDF_TEST_ENV_HH + +#include +#include + +#ifdef _WIN32 +#include +#endif + +namespace sdf +{ + namespace testing + { + + /// \brief Find the environment variable '_name' and return its value. + /// + /// Note: the intention is to put this in ign-util, and remove it + /// from sdformat once the depcnency is in place + /// + /// \param[in] _name Name of the environment variable. + /// \param[out] _value Value if the variable was found. + /// \param[in] _allowEmpty Allow set-but-empty variables. + /// (Unsupported on Windows) + /// \return True if the variable was found or false otherwise. + bool env(const std::string &_name, + std::string &_value, + bool _allowEmpty) + { + std::string v; + bool valid = false; +#ifdef _WIN32 + // Unused on Windows, suppress warning + (void) _allowEmpty; + const DWORD buffSize = 32767; + static char buffer[buffSize]; + if (GetEnvironmentVariable(_name.c_str(), buffer, buffSize)) + { + v = buffer; + } + + if (!v.empty()) + { + valid = true; + } + +#else + const char *cvar = std::getenv(_name.c_str()); + if (cvar != nullptr) + { + v = cvar; + valid = true; + + if (v[0] == '\0' && !_allowEmpty) + { + valid = false; + } + } +#endif + if (valid) + { + _value = v; + return true; + } + return false; + } + + bool env(const std::string &_name, std::string &_value) + { + return env(_name, _value, false); + } + + /// \brief Set the environment variable '_name'. + /// + /// Note that on Windows setting an empty string (_value=="") + /// is the equivalent of unsetting the variable. + /// + /// \param[in] _name Name of the environment variable. + /// \param[in] _value Value of the variable to be set. + /// \return True if the variable was set or false otherwise. + bool setenv(const std::string &_name, + const std::string &_value) + { +#ifdef _WIN32 + if (0 != _putenv_s(_name.c_str(), _value.c_str())) + { + return false; + } +#else + if (0 != ::setenv(_name.c_str(), _value.c_str(), true)) + { + return false; + } +#endif + return true; + } + + /// \brief Unset the environment variable '_name'. + /// \param[in] _name Name of the environment variable. + /// \return True if the variable was unset or false otherwise. + bool unsetenv(const std::string &_name) + { +#ifdef _WIN32 + if (0 != _putenv_s(_name.c_str(), "")) + { + return false; + } +#else + if (0 != ::unsetenv(_name.c_str())) + { + return false; + } +#endif + return true; + } + } // namespace testing +} // namespace sdf + +#endif diff --git a/test/integration/actor_dom.cc b/test/integration/actor_dom.cc index 500a5d032..f4a53e6b0 100644 --- a/test/integration/actor_dom.cc +++ b/test/integration/actor_dom.cc @@ -31,8 +31,7 @@ TEST(DOMActor, LoadActors) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "world_complete.sdf"); + sdf::testing::TestFile("sdf", "world_complete.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); @@ -145,8 +144,7 @@ TEST(DOMActor, CopySdfLoadedProperties) // Verify that copying an actor also copies the underlying ElementPtr // Joints and Links const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "world_complete.sdf"); + sdf::testing::TestFile("sdf", "world_complete.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); diff --git a/test/integration/audio.cc b/test/integration/audio.cc index b8e7e09b4..57a5bd642 100644 --- a/test/integration/audio.cc +++ b/test/integration/audio.cc @@ -23,12 +23,13 @@ #include "test_config.h" - +////////////////////////////////////////////////// TEST(SDFParser, AudioSDF_FullParameters_noThrow) { - const std::string sdfTestFile = sdf::filesystem::append( - PROJECT_SOURCE_PATH, "test", "integration", "audio.sdf"); + const auto sdfTestFile = + sdf::testing::TestFile("integration", "audio.sdf"); + sdf::SDFPtr p(new sdf::SDF()); sdf::init(p); - ASSERT_TRUE(sdf::readFile(sdfTestFile , p)); + ASSERT_TRUE(sdf::readFile(sdfTestFile, p)); } diff --git a/test/integration/cfm_damping_implicit_spring_damper.cc b/test/integration/cfm_damping_implicit_spring_damper.cc index 2c5cc090e..5112e1366 100644 --- a/test/integration/cfm_damping_implicit_spring_damper.cc +++ b/test/integration/cfm_damping_implicit_spring_damper.cc @@ -27,9 +27,8 @@ ///////////////////////////////////////////////// TEST(SDFParser, CFMDampingSDFTest) { - const std::string sdfTestFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "cfm_damping_implicit_spring_damper.sdf"); + const std::string sdfTestFile = sdf::testing::TestFile( + "integration", "cfm_damping_implicit_spring_damper.sdf"); sdf::SDFPtr robot(new sdf::SDF()); sdf::init(robot); ASSERT_TRUE(sdf::readFile(sdfTestFile, robot)); @@ -107,9 +106,8 @@ TEST(SDFParser, CFMDampingSDFTest) ///////////////////////////////////////////////// TEST(SDFParser, CFMDampingURDFTest) { - const std::string urdfTestFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "cfm_damping_implicit_spring_damper.urdf"); + const std::string urdfTestFile = sdf::testing::TestFile( + "integration", "cfm_damping_implicit_spring_damper.urdf"); sdf::SDFPtr robot(new sdf::SDF()); sdf::init(robot); diff --git a/test/integration/collision_dom.cc b/test/integration/collision_dom.cc index 06ea0b23e..08d48fc0b 100644 --- a/test/integration/collision_dom.cc +++ b/test/integration/collision_dom.cc @@ -64,8 +64,7 @@ TEST(DOMCollision, NoName) TEST(DOMCollision, DoublePendulum) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "double_pendulum.sdf"); + sdf::testing::TestFile("sdf", "double_pendulum.sdf"); // Load the SDF file sdf::Root root; @@ -95,8 +94,7 @@ TEST(DOMCollision, DoublePendulum) TEST(DOMCollision, LoadModelFramesRelativeToJoint) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_frame_relative_to_joint.sdf"); + sdf::testing::TestFile("sdf", "model_frame_relative_to_joint.sdf"); // Load the SDF file sdf::Root root; diff --git a/test/integration/converter.cc b/test/integration/converter.cc index 00629be72..73c13ffbe 100644 --- a/test/integration/converter.cc +++ b/test/integration/converter.cc @@ -31,8 +31,7 @@ void ParserStringConverter(const std::string &_version); /// Test conversion using the parser sdf file converter interface. TEST(ConverterIntegration, ParserFileConverter) { - std::string filename = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", - "integration", "audio.sdf"); + const auto filename = sdf::testing::TestFile("integration", "audio.sdf"); sdf::SDFPtr sdf(new sdf::SDF()); sdf::init(sdf); @@ -73,8 +72,8 @@ TEST(ConverterIntegration, ParserFileConverter) /// Convert to a previous SDF version TEST(ConverterIntegration, convertFileToNotLatestVersion) { - std::string filename = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", - "integration", "audio.sdf"); + const auto filename = sdf::testing::TestFile( + "integration", "audio.sdf"); sdf::SDFPtr sdf(new sdf::SDF()); sdf::init(sdf); diff --git a/test/integration/disable_fixed_joint_reduction.cc b/test/integration/disable_fixed_joint_reduction.cc index 383c3d24c..8d4cd7f4a 100644 --- a/test/integration/disable_fixed_joint_reduction.cc +++ b/test/integration/disable_fixed_joint_reduction.cc @@ -45,12 +45,11 @@ bool findJointInModel(const std::string &desired_joint_name, sdf::SDFPtr robot) ///////////////////////////////////////////////// TEST(SDFParser, DisableFixedJointReductionTest) { - const std::string sdfFixedJntFile = sdf::filesystem::append( - PROJECT_SOURCE_PATH, "test", "integration", "fixed_joint_reduction.urdf"); + const std::string sdfFixedJntFile = + sdf::testing::TestFile("integration", "fixed_joint_reduction.urdf"); - const std::string sdfFixedJntNoLumpingFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "fixed_joint_reduction_disabled.urdf"); + const std::string sdfFixedJntNoLumpingFile = sdf::testing::TestFile( + "integration", "fixed_joint_reduction_disabled.urdf"); sdf::SDFPtr robot(new sdf::SDF()); sdf::init(robot); diff --git a/test/integration/element_memory_leak.cc b/test/integration/element_memory_leak.cc index 00caa6408..fb877e273 100644 --- a/test/integration/element_memory_leak.cc +++ b/test/integration/element_memory_leak.cc @@ -72,7 +72,7 @@ const std::string sdfString( int getMemoryUsage() { static const std::string getMemInfoPath = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "tools", "get_mem_info.py"); + sdf::testing::SourceFile("tools", "get_mem_info.py"); static const std::string pythonMeminfo("python3 " + getMemInfoPath); return std::stoi(custom_exec(pythonMeminfo)); diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index 0b1f828d1..0475db78b 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -24,14 +24,10 @@ #include "test_config.h" -const char SDF_TEST_FILE[] = - "fixed_joint_reduction.urdf"; -const char SDF_TEST_FILE_COLLISION[] = - "fixed_joint_reduction_collision.urdf"; -const char SDF_TEST_FILE_SIMPLE[] = - "fixed_joint_reduction_simple.urdf"; -const char SDF_TEST_FILE_VISUAL[] = - "fixed_joint_reduction_visual.urdf"; +const char SDF_TEST_FILE[] = "fixed_joint_reduction.urdf"; +const char SDF_TEST_FILE_COLLISION[] = "fixed_joint_reduction_collision.urdf"; +const char SDF_TEST_FILE_SIMPLE[] = "fixed_joint_reduction_simple.urdf"; +const char SDF_TEST_FILE_VISUAL[] = "fixed_joint_reduction_visual.urdf"; const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION[] = "fixed_joint_reduction_collision_visual_extension.urdf"; const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_SDF[] = @@ -43,8 +39,7 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] = static std::string GetFullTestFilePath(const char *_input) { - return sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - _input); + return sdf::testing::TestFile("integration", _input); } const double gc_tolerance = 1e-6; diff --git a/test/integration/force_torque_sensor.cc b/test/integration/force_torque_sensor.cc index 12dc60ce1..f5e2f18b9 100644 --- a/test/integration/force_torque_sensor.cc +++ b/test/integration/force_torque_sensor.cc @@ -26,8 +26,8 @@ ///////////////////////////////////////////////// TEST(SDFParser, ForceTorqueSensorTest) { - const std::string sdfTestFile = sdf::filesystem::append( - PROJECT_SOURCE_PATH, "test", "integration", "force_torque_sensor.urdf"); + const std::string sdfTestFile = + sdf::testing::TestFile("integration", "force_torque_sensor.urdf"); sdf::SDFPtr robot(new sdf::SDF()); sdf::init(robot); diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 52238aa14..53fc09237 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -301,8 +301,7 @@ TEST(Frame, StateFrame) TEST(Frame, IncludeRelativeTo) { const std::string MODEL_PATH = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "model", "box"); + sdf::testing::TestFile("integration", "model", "box"); std::ostringstream stream; std::string version = SDF_VERSION; @@ -348,8 +347,7 @@ TEST(Frame, IncludeRelativeTo) TEST(Frame, IncludeRelativeToEmptyPose) { const std::string MODEL_PATH = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "model", "box"); + sdf::testing::TestFile("integration", "model", "box"); std::ostringstream stream; std::string version = SDF_VERSION; @@ -415,8 +413,7 @@ TEST(Frame, IncludeRelativeToEmptyPose) TEST(DOMFrame, LoadModelFramesAttachedTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_frame_attached_to.sdf"); + sdf::testing::TestFile("sdf", "model_frame_attached_to.sdf"); // Load the SDF file sdf::Root root; @@ -475,7 +472,7 @@ TEST(DOMFrame, LoadModelFramesAttachedTo) TEST(DOMFrame, LoadModelFramesInvalidAttachedTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "model_frame_invalid_attached_to.sdf"); // Load the SDF file @@ -577,7 +574,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo) TEST(DOMFrame, LoadModelFramesAttachedToJoint) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "model_frame_attached_to_joint.sdf"); // Load the SDF file @@ -642,7 +639,7 @@ TEST(DOMFrame, LoadModelFramesAttachedToJoint) TEST(DOMFrame, LoadModelFramesAttachedToNestedModel) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "model_frame_attached_to_nested_model.sdf"); // Load the SDF file @@ -691,7 +688,7 @@ TEST(DOMFrame, LoadModelFramesAttachedToNestedModel) TEST(DOMFrame, LoadWorldFramesAttachedTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "world_frame_attached_to.sdf"); // Load the SDF file @@ -758,7 +755,7 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo) TEST(DOMFrame, LoadWorldFramesInvalidAttachedTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "world_frame_invalid_attached_to.sdf"); // Load the SDF file @@ -862,7 +859,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidAttachedTo) TEST(DOMFrame, LoadModelFramesRelativeTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "model_frame_relative_to.sdf"); // Load the SDF file @@ -995,7 +992,7 @@ TEST(DOMFrame, LoadModelFramesRelativeTo) TEST(DOMFrame, LoadModelFramesInvalidRelativeTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "model_invalid_frame_relative_to.sdf"); // Load the SDF file @@ -1023,7 +1020,7 @@ TEST(DOMFrame, LoadModelFramesInvalidRelativeTo) TEST(DOMFrame, LoadModelFramesRelativeToJoint) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "model_frame_relative_to_joint.sdf"); // Load the SDF file @@ -1157,7 +1154,7 @@ TEST(DOMFrame, LoadModelFramesRelativeToJoint) TEST(DOMFrame, LoadWorldFramesRelativeTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "world_frame_relative_to.sdf"); // Load the SDF file @@ -1223,7 +1220,7 @@ TEST(DOMFrame, LoadWorldFramesRelativeTo) TEST(DOMFrame, LoadWorldFramesInvalidRelativeTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "world_frame_invalid_relative_to.sdf"); // Load the SDF file @@ -1249,8 +1246,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidRelativeTo) TEST(DOMFrame, WorldIncludeModel) { const std::string MODEL_PATH = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "model", "box"); + sdf::testing::TestFile("integration", "model", "box"); std::ostringstream stream; std::string version = SDF_VERSION; diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 9955c3ab1..06adebb1e 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -34,14 +34,13 @@ #include "sdf/Types.hh" #include "sdf/Visual.hh" #include "sdf/World.hh" + #include "test_config.h" ////////////////////////////////////////////////// TEST(DOMGeometry, Shapes) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "shapes.sdf"); + const auto testFile = sdf::testing::TestFile("sdf", "shapes.sdf"); // Load the SDF file sdf::Root root; diff --git a/test/integration/include.cc b/test/integration/include.cc index b7f3304fa..02b1f2b8e 100644 --- a/test/integration/include.cc +++ b/test/integration/include.cc @@ -29,8 +29,7 @@ TEST(Include, IncludeDescription) { const std::string SDF_DESCRIPTION_PATH = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "include_description.sdf"); + sdf::testing::TestFile("integration", "include_description.sdf"); std::ostringstream stream; std::string version = SDF_VERSION; diff --git a/test/integration/includes.cc b/test/integration/includes.cc index 77862cfcd..815643ed9 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -34,14 +34,10 @@ #include "sdf/World.hh" #include "test_config.h" -const auto g_testPath = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test"); -const auto g_modelsPath = - sdf::filesystem::append(g_testPath, "integration", "model"); - ///////////////////////////////////////////////// std::string findFileCb(const std::string &_input) { - return sdf::filesystem::append(g_testPath, "integration", "model", _input); + return sdf::testing::TestFile("integration", "model", _input); } ////////////////////////////////////////////////// @@ -49,8 +45,7 @@ TEST(IncludesTest, Includes) { sdf::setFindCallback(findFileCb); - const auto worldFile = - sdf::filesystem::append(g_testPath, "sdf", "includes.sdf"); + const auto worldFile = sdf::testing::TestFile("sdf", "includes.sdf"); sdf::Root root; sdf::Errors errors = root.Load(worldFile); @@ -77,8 +72,8 @@ TEST(IncludesTest, Includes) const auto *actor = world->ActorByIndex(0); EXPECT_EQ("1.6", actor->Element()->OriginalVersion()); - const auto actorFile = sdf::filesystem::append(g_modelsPath, "test_actor", - "model.sdf"); + const auto actorFile = sdf::testing::TestFile( + "integration", "model", "test_actor", "model.sdf"); EXPECT_EQ(actorFile, actor->FilePath()); EXPECT_EQ("actor", actor->Name()); @@ -166,8 +161,8 @@ TEST(IncludesTest, Includes) EXPECT_FALSE(model->LinkNameExists("coconut")); EXPECT_EQ("1.6", model->Element()->OriginalVersion()); - const auto modelFile = sdf::filesystem::append(g_modelsPath, "test_model", - "model.sdf"); + const auto modelFile = sdf::testing::TestFile( + "integration", "model", "test_model", "model.sdf"); const auto *link = model->LinkByName("link"); ASSERT_NE(nullptr, link); @@ -216,8 +211,7 @@ TEST(IncludesTest, Includes_15) { sdf::setFindCallback(findFileCb); - const auto worldFile = - sdf::filesystem::append(g_testPath, "sdf", "includes_1.5.sdf"); + const auto worldFile = sdf::testing::TestFile("sdf", "includes_1.5.sdf"); sdf::Root root; sdf::Errors errors = root.Load(worldFile); @@ -273,8 +267,7 @@ TEST(IncludesTest, Includes_15_convert) { sdf::setFindCallback(findFileCb); - const auto worldFile = - sdf::filesystem::append(g_testPath, "sdf", "includes_1.5.sdf"); + const auto worldFile = sdf::testing::TestFile("sdf", "includes_1.5.sdf"); sdf::SDFPtr sdf(new sdf::SDF()); sdf::init(sdf); diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc index 24d6fbd10..5169557f3 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -33,8 +33,7 @@ TEST(DOMJointAxis, Complete) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "joint_complete.sdf"); + sdf::testing::TestFile("sdf", "joint_complete.sdf"); // Load the SDF file sdf::Root root; @@ -114,8 +113,7 @@ TEST(DOMJointAxis, Complete) TEST(DOMJointAxis, XyzExpressedIn) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_joint_axis_expressed_in.sdf"); + sdf::testing::TestFile("sdf", "model_joint_axis_expressed_in.sdf"); // Load the SDF file sdf::Root root; diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 255a0e0fc..379b644b1 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -63,8 +63,7 @@ TEST(DOMJoint, NoName) TEST(DOMJoint, DoublePendulum) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "double_pendulum.sdf"); + sdf::testing::TestFile("sdf", "double_pendulum.sdf"); // Load the SDF file sdf::Root root; @@ -117,8 +116,7 @@ TEST(DOMJoint, DoublePendulum) TEST(DOMJoint, Complete) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "joint_complete.sdf"); + sdf::testing::TestFile("sdf", "joint_complete.sdf"); // Load the SDF file sdf::Root root; @@ -160,8 +158,7 @@ TEST(DOMJoint, Complete) TEST(DOMJoint, LoadJointParentWorld) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "joint_parent_world.sdf"); + sdf::testing::TestFile("sdf", "joint_parent_world.sdf"); // Load the SDF file sdf::Root root; @@ -204,8 +201,7 @@ TEST(DOMJoint, LoadJointParentWorld) TEST(DOMJoint, LoadInvalidJointChildWorld) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "joint_child_world.sdf"); + sdf::testing::TestFile("sdf", "joint_child_world.sdf"); // Load the SDF file sdf::Root root; @@ -229,8 +225,7 @@ TEST(DOMJoint, LoadInvalidJointChildWorld) TEST(DOMJoint, LoadJointPoseRelativeTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_joint_relative_to.sdf"); + sdf::testing::TestFile("sdf", "model_joint_relative_to.sdf"); // Load the SDF file sdf::Root root; @@ -324,7 +319,7 @@ TEST(DOMJoint, LoadJointPoseRelativeTo) TEST(DOMJoint, LoadInvalidJointPoseRelativeTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "model_invalid_joint_relative_to.sdf"); // Load the SDF file @@ -353,8 +348,7 @@ TEST(DOMJoint, LoadInvalidJointPoseRelativeTo) TEST(DOMJoint, LoadInvalidChild) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "joint_invalid_child.sdf"); + sdf::testing::TestFile("sdf", "joint_invalid_child.sdf"); // Load the SDF file sdf::Root root; @@ -383,7 +377,7 @@ TEST(DOMJoint, LoadInvalidChild) TEST(DOMJoint, LoadLinkJointSameName17Invalid) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "model_link_joint_same_name.sdf"); // Read with sdf::readFile, which converts from 1.6 to latest @@ -414,8 +408,7 @@ TEST(DOMJoint, LoadLinkJointSameName17Invalid) TEST(DOMJoint, LoadLinkJointSameName16Valid) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_link_joint_same_name.sdf"); + sdf::testing::TestFile("sdf", "model_link_joint_same_name.sdf"); // Load the SDF file sdf::Root root; @@ -493,8 +486,7 @@ TEST(DOMJoint, LoadLinkJointSameName16Valid) TEST(DOMJoint, LoadURDFJointPoseRelativeTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "provide_feedback.urdf"); + sdf::testing::TestFile("integration", "provide_feedback.urdf"); // Load the SDF file sdf::Root root; diff --git a/test/integration/light_dom.cc b/test/integration/light_dom.cc index e2d4e8396..5e3d26ab4 100644 --- a/test/integration/light_dom.cc +++ b/test/integration/light_dom.cc @@ -33,8 +33,7 @@ TEST(DOMWorld, LoadLights) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "world_complete.sdf"); + sdf::testing::TestFile("sdf", "world_complete.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 4597783e9..506c868a6 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -73,8 +73,7 @@ TEST(DOMLink, NoName) TEST(DOMLink, LoadVisualCollision) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty.sdf"); + sdf::testing::TestFile("sdf", "empty.sdf"); // Load the SDF file sdf::Root root; @@ -116,8 +115,7 @@ TEST(DOMLink, LoadVisualCollision) TEST(DOMLink, InertialDoublePendulum) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "double_pendulum.sdf"); + sdf::testing::TestFile("sdf", "double_pendulum.sdf"); // Load the SDF file sdf::Root root; @@ -171,8 +169,7 @@ TEST(DOMLink, InertialDoublePendulum) TEST(DOMLink, InertialComplete) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "inertial_complete.sdf"); + sdf::testing::TestFile("sdf", "inertial_complete.sdf"); // Load the SDF file sdf::Root root; @@ -203,8 +200,7 @@ TEST(DOMLink, InertialComplete) TEST(DOMLink, InertialInvalid) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "inertial_invalid.sdf"); + sdf::testing::TestFile("sdf", "inertial_invalid.sdf"); // Load the SDF file sdf::Root root; @@ -228,9 +224,7 @@ TEST(DOMLink, InertialInvalid) ////////////////////////////////////////////////// TEST(DOMLink, Sensors) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "sensors.sdf"); + const std::string testFile = sdf::testing::TestFile("sdf", "sensors.sdf"); // Load the SDF file sdf::Root root; @@ -615,8 +609,7 @@ TEST(DOMLink, Sensors) ///////////////////////////////////////////////// TEST(DOMLink, LoadLinkPoseRelativeTo) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + const std::string testFile = sdf::testing::TestFile("sdf", "model_link_relative_to.sdf"); // Load the SDF file @@ -692,8 +685,7 @@ TEST(DOMLink, LoadLinkPoseRelativeTo) ///////////////////////////////////////////////// TEST(DOMLink, LoadInvalidLinkPoseRelativeTo) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + const std::string testFile = sdf::testing::TestFile("sdf", "model_invalid_link_relative_to.sdf"); // Load the SDF file diff --git a/test/integration/material_pbr.cc b/test/integration/material_pbr.cc index 1d684deea..a315abac6 100644 --- a/test/integration/material_pbr.cc +++ b/test/integration/material_pbr.cc @@ -26,8 +26,7 @@ TEST(Material, PbrDOM) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "material_pbr.sdf"); + sdf::testing::TestFile("sdf", "material_pbr.sdf"); // Load the SDF file into DOM sdf::Root root; @@ -261,8 +260,7 @@ TEST(Material, PbrDOM) TEST(Material, MaterialPBR) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "material_pbr.sdf"); + sdf::testing::TestFile("sdf", "material_pbr.sdf"); // load material pbr sdf file sdf::Errors errors; diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 6bab729d4..8666cad60 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -63,8 +63,7 @@ TEST(DOMModel, NoName) TEST(DOMModel, NoLinks) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_without_links.sdf"); + sdf::testing::TestFile("sdf", "model_without_links.sdf"); // Load the SDF file sdf::Root root; @@ -85,8 +84,7 @@ TEST(DOMModel, NoLinks) TEST(DOMRoot, LoadLinkCheck) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty.sdf"); + sdf::testing::TestFile("sdf", "empty.sdf"); // Load the SDF file sdf::Root root; @@ -114,8 +112,7 @@ TEST(DOMRoot, LoadLinkCheck) TEST(DOMRoot, LoadDoublePendulum) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "double_pendulum.sdf"); + sdf::testing::TestFile("sdf", "double_pendulum.sdf"); // Load the SDF file sdf::Root root; @@ -153,8 +150,7 @@ TEST(DOMRoot, LoadDoublePendulum) TEST(DOMRoot, NestedModel) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "nested_model.sdf"); + sdf::testing::TestFile("sdf", "nested_model.sdf"); // Load the SDF file sdf::Root root; @@ -206,7 +202,7 @@ TEST(DOMRoot, NestedModel) TEST(DOMLink, NestedModelPoseRelativeTo) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + sdf::testing::TestFile("sdf", "model_nested_model_relative_to.sdf"); // Load the SDF file @@ -289,8 +285,7 @@ TEST(DOMLink, NestedModelPoseRelativeTo) TEST(DOMRoot, LoadCanonicalLink) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_canonical_link.sdf"); + sdf::testing::TestFile("sdf", "model_canonical_link.sdf"); // Load the SDF file sdf::Root root; @@ -331,8 +326,7 @@ TEST(DOMRoot, LoadCanonicalLink) TEST(DOMRoot, LoadNestedCanonicalLink) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "nested_canonical_link.sdf"); + sdf::testing::TestFile("sdf", "nested_canonical_link.sdf"); // Load the SDF file sdf::Root root; diff --git a/test/integration/model_versions.cc b/test/integration/model_versions.cc index ebadb3c67..a091a598b 100644 --- a/test/integration/model_versions.cc +++ b/test/integration/model_versions.cc @@ -32,8 +32,8 @@ TEST(ModelVersionsTest, Empty_ModelFilePath) TEST(ModelVersionsTest, NonExistent_ModelFilePath) { - const std::string MODEL_PATH = sdf::filesystem::append(PROJECT_SOURCE_PATH, - "test", "integration", "model", "non-existent"); + const std::string MODEL_PATH = + sdf::testing::TestFile("integration", "model", "non-existent"); std::string modelPath = sdf::getModelFilePath(MODEL_PATH); @@ -42,8 +42,8 @@ TEST(ModelVersionsTest, NonExistent_ModelFilePath) TEST(ModelVersionsTest, MalFormed_ModelFilePath) { - const std::string MODEL_PATH = sdf::filesystem::append(PROJECT_SOURCE_PATH, - "test", "integration", "model", "cococan_malformed"); + const std::string MODEL_PATH = + sdf::testing::TestFile("integration", "model", "cococan_malformed"); std::string modelPath = sdf::getModelFilePath(MODEL_PATH); @@ -52,8 +52,8 @@ TEST(ModelVersionsTest, MalFormed_ModelFilePath) TEST(ModelVersionsTest, NoVersionTag_ModelFilePath) { - const std::string MODEL_PATH = sdf::filesystem::append(PROJECT_SOURCE_PATH, - "test", "integration", "model", "cococan_noversiontag"); + const std::string MODEL_PATH = + sdf::testing::TestFile("integration", "model", "cococan_noversiontag"); std::string modelPath = sdf::getModelFilePath(MODEL_PATH); @@ -62,8 +62,8 @@ TEST(ModelVersionsTest, NoVersionTag_ModelFilePath) TEST(ModelVersionsTest, Correct_ModelFilePath) { - const std::string MODEL_PATH = sdf::filesystem::append(PROJECT_SOURCE_PATH, - "test", "integration", "model", "cococan"); + const std::string MODEL_PATH = + sdf::testing::TestFile("integration", "model", "cococan"); std::string modelPath = sdf::getModelFilePath(MODEL_PATH); diff --git a/test/integration/plugin_include.cc b/test/integration/plugin_include.cc index d39d64d0b..4ad0420bc 100644 --- a/test/integration/plugin_include.cc +++ b/test/integration/plugin_include.cc @@ -25,8 +25,8 @@ // Test that plugin child elements are available even when nested in an include TEST(PluginInclude, PluginChildElements) { - const std::string MODEL_PATH = std::string(PROJECT_SOURCE_PATH) - + "/test/integration/model/box"; + const auto MODEL_PATH = + sdf::testing::TestFile("integration", "model", "box"); std::ostringstream stream; stream @@ -80,8 +80,8 @@ TEST(PluginInclude, PluginChildElements) // Test that missing required plugin attributes are detected TEST(PluginInclude, PluginMissingFilename) { - const std::string MODEL_PATH = std::string(PROJECT_SOURCE_PATH) - + "/test/integration/model/box"; + const auto MODEL_PATH = + sdf::testing::TestFile("integration", "model", "box"); std::ostringstream stream; stream diff --git a/test/integration/provide_feedback.cc b/test/integration/provide_feedback.cc index e8ed95991..582a39c3f 100644 --- a/test/integration/provide_feedback.cc +++ b/test/integration/provide_feedback.cc @@ -26,8 +26,8 @@ ///////////////////////////////////////////////// TEST(SDFParser, ProvideFeedbackTest) { - const std::string sdfTestFile = sdf::filesystem::append( - PROJECT_SOURCE_PATH, "test", "integration", "provide_feedback.urdf"); + const std::string sdfTestFile = + sdf::testing::TestFile("integration", "provide_feedback.urdf"); sdf::SDFPtr robot(new sdf::SDF()); sdf::init(robot); diff --git a/test/integration/root_dom.cc b/test/integration/root_dom.cc index d28e154c3..b5f437def 100644 --- a/test/integration/root_dom.cc +++ b/test/integration/root_dom.cc @@ -30,8 +30,7 @@ TEST(DOMRoot, InvalidSDF) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty_invalid.sdf"); + sdf::testing::TestFile("sdf", "empty_invalid.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); @@ -43,8 +42,7 @@ TEST(DOMRoot, InvalidSDF) TEST(DOMRoot, NoVersion) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty_noversion.sdf"); + sdf::testing::TestFile("sdf", "empty_noversion.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); @@ -56,8 +54,7 @@ TEST(DOMRoot, NoVersion) TEST(DOMRoot, Load) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty.sdf"); + sdf::testing::TestFile("sdf", "empty.sdf"); sdf::Root root; EXPECT_EQ(0u, root.WorldCount()); @@ -79,8 +76,7 @@ TEST(DOMRoot, Load) TEST(DOMRoot, LoadMultipleModels) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "root_multiple_models.sdf"); + sdf::testing::TestFile("sdf", "root_multiple_models.sdf"); sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); @@ -100,8 +96,7 @@ TEST(DOMRoot, LoadMultipleModels) TEST(DOMRoot, LoadDuplicateModels) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "root_duplicate_models.sdf"); + sdf::testing::TestFile("sdf", "root_duplicate_models.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); diff --git a/test/integration/scene_dom.cc b/test/integration/scene_dom.cc index 84ae92805..edcee1acb 100644 --- a/test/integration/scene_dom.cc +++ b/test/integration/scene_dom.cc @@ -35,8 +35,7 @@ TEST(DOMScene, LoadScene) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "scene_with_sky.sdf"); + sdf::testing::TestFile("sdf", "scene_with_sky.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); diff --git a/test/integration/sdf_custom.cc b/test/integration/sdf_custom.cc index 63402c5ff..2b29d4f67 100644 --- a/test/integration/sdf_custom.cc +++ b/test/integration/sdf_custom.cc @@ -26,8 +26,8 @@ ///////////////////////////////////////////////// TEST(SDFParser, CustomElements) { - const std::string sdfTestFile = sdf::filesystem::append( - PROJECT_SOURCE_PATH, "test", "integration", "custom_elems_attrs.sdf"); + const std::string sdfTestFile = + sdf::testing::TestFile("integration", "custom_elems_attrs.sdf"); sdf::Root root; EXPECT_TRUE(root.Load(sdfTestFile).empty()); diff --git a/test/integration/surface_dom.cc b/test/integration/surface_dom.cc index 8363704f8..d866d898f 100644 --- a/test/integration/surface_dom.cc +++ b/test/integration/surface_dom.cc @@ -27,14 +27,14 @@ #include "sdf/Surface.hh" #include "sdf/Types.hh" #include "sdf/World.hh" + #include "test_config.h" ////////////////////////////////////////////////// TEST(DOMSurface, Shapes) { const auto testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "shapes.sdf"); + sdf::testing::TestFile("sdf", "shapes.sdf"); sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 80b064c8f..36d0d8603 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -27,8 +27,7 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) { const std::string urdfTestFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "urdf_gazebo_extensions.urdf"); + sdf::testing::TestFile("integration", "urdf_gazebo_extensions.urdf"); sdf::SDFPtr robot(new sdf::SDF()); sdf::init(robot); diff --git a/test/integration/urdf_joint_parameters.cc b/test/integration/urdf_joint_parameters.cc index 7a92d8a6e..e6ae97d87 100644 --- a/test/integration/urdf_joint_parameters.cc +++ b/test/integration/urdf_joint_parameters.cc @@ -26,8 +26,8 @@ ///////////////////////////////////////////////// TEST(SDFParser, JointAxisParameters) { - const std::string sdfTestFile = sdf::filesystem::append( - PROJECT_SOURCE_PATH, "test", "integration", "urdf_joint_parameters.urdf"); + const std::string sdfTestFile = + sdf::testing::TestFile("integration", "urdf_joint_parameters.urdf"); sdf::SDFPtr robot(new sdf::SDF()); sdf::init(robot); diff --git a/test/integration/visual_dom.cc b/test/integration/visual_dom.cc index df124f51b..e882b803a 100644 --- a/test/integration/visual_dom.cc +++ b/test/integration/visual_dom.cc @@ -64,8 +64,7 @@ TEST(DOMVisual, NoName) TEST(DOMVisual, DoublePendulum) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "double_pendulum.sdf"); + sdf::testing::TestFile("sdf", "double_pendulum.sdf"); // Load the SDF file sdf::Root root; @@ -97,8 +96,7 @@ TEST(DOMVisual, DoublePendulum) TEST(DOMVisual, Material) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "material.sdf"); + sdf::testing::TestFile("sdf", "material.sdf"); // Load the SDF file sdf::Root root; @@ -146,8 +144,7 @@ TEST(DOMVisual, Material) TEST(DOMVisual, MaterialScriptNoUri) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "material_script_no_uri.sdf"); + sdf::testing::TestFile("sdf", "material_script_no_uri.sdf"); // Load the SDF file sdf::Root root; @@ -167,8 +164,7 @@ TEST(DOMVisual, MaterialScriptNoUri) TEST(DOMVisual, MaterialScriptNormalMapMissing) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "material_normal_map_missing.sdf"); + sdf::testing::TestFile("sdf", "material_normal_map_missing.sdf"); // Load the SDF file sdf::Root root; @@ -184,8 +180,7 @@ TEST(DOMVisual, MaterialScriptNormalMapMissing) TEST(DOMVisual, Transparency) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "shapes.sdf"); + sdf::testing::TestFile("sdf", "shapes.sdf"); // Load the SDF file sdf::Root root; @@ -231,8 +226,7 @@ TEST(DOMVisual, LaserRetro) TEST(DOMVisual, LoadModelFramesRelativeToJoint) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_frame_relative_to_joint.sdf"); + sdf::testing::TestFile("sdf", "model_frame_relative_to_joint.sdf"); // Load the SDF file sdf::Root root; @@ -425,8 +419,7 @@ TEST(DOMVisual, LoadModelFramesRelativeToJoint) TEST(DOMVisual, VisibilityFlags) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "shapes.sdf"); + sdf::testing::TestFile("sdf", "shapes.sdf"); // Load the SDF file sdf::Root root; diff --git a/test/integration/world_dom.cc b/test/integration/world_dom.cc index 878a74f0b..5a529d1a4 100644 --- a/test/integration/world_dom.cc +++ b/test/integration/world_dom.cc @@ -25,15 +25,13 @@ #include "sdf/Model.hh" #include "sdf/Root.hh" #include "sdf/World.hh" -#include "sdf/Filesystem.hh" #include "test_config.h" ////////////////////////////////////////////////// TEST(DOMWorld, NoName) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "world_noname.sdf"); + sdf::testing::TestFile("sdf", "world_noname.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); @@ -46,8 +44,7 @@ TEST(DOMWorld, NoName) TEST(DOMWorld, Duplicate) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "world_duplicate.sdf"); + sdf::testing::TestFile("sdf", "world_duplicate.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); @@ -59,8 +56,7 @@ TEST(DOMWorld, Duplicate) TEST(DOMWorld, LoadIncorrectElement) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "world_complete.sdf"); + sdf::testing::TestFile("sdf", "world_complete.sdf"); sdf::Errors errors; // Read an SDF file, and store the result in sdfParsed. @@ -82,8 +78,7 @@ TEST(DOMWorld, LoadIncorrectElement) TEST(DOMWorld, Load) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "world_complete.sdf"); + sdf::testing::TestFile("sdf", "world_complete.sdf"); sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); @@ -152,8 +147,7 @@ TEST(DOMWorld, Load) TEST(DOMWorld, LoadModelFrameSameName) { const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "world_model_frame_same_name.sdf"); + sdf::testing::TestFile("sdf", "world_model_frame_same_name.sdf"); // Load the SDF file sdf::Root root; diff --git a/test/performance/parser_urdf.cc b/test/performance/parser_urdf.cc index 2fbe18284..ac25e56a6 100644 --- a/test/performance/parser_urdf.cc +++ b/test/performance/parser_urdf.cc @@ -26,10 +26,9 @@ TEST(URDFParser, AtlasURDF_5runs_performance) { - const std::string - URDF_TEST_FILE = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", - "performance", - "parser_urdf_atlas.urdf"); + const std::string URDF_TEST_FILE = + sdf::testing::TestFile("performance", "parser_urdf_atlas.urdf"); + for (int i = 0; i < 5; i++) { sdf::SDFPtr root = sdf::readFile(URDF_TEST_FILE); diff --git a/test/test_config.h.in b/test/test_config.h.in index 6153b85c8..0342e73a9 100644 --- a/test/test_config.h.in +++ b/test/test_config.h.in @@ -25,27 +25,104 @@ #define PROJECT_SOURCE_PATH "${PROJECT_SOURCE_DIR}" #define PROJECT_BINARY_DIR "${CMAKE_BINARY_DIR}" #define SDF_PROTOCOL_VERSION "${SDF_PROTOCOL_VERSION}" +#define SDF_TMP_DIR "tmp-sdf/" -/* - * setenv/unstenv are not present in Windows. Define them to make the code - * portable - */ -#if (_MSC_VER >= 1400) // Visual Studio 2005 -#include +#include "env.hh" -int setenv(const char * name, const char * value, int /*rewrite*/) +#include +namespace sdf { - std::stringstream sstr; - sstr << *name << '=' << value; - return _putenv(sstr.str().c_str()); -} + namespace testing + { + /// \brief Method to retrieve root directory of project source + /// + /// This is used to get various test files + /// \param[inout] _sourceDir Full path to the source directory + /// \return True if directory is set correctly, false otherwise + bool ProjectSourcePath(std::string &_sourceDir) + { + // Bazel builds set TEST_SRCDIR + if(env("TEST_SRCDIR", _sourceDir)) + { + _sourceDir = sdf::filesystem::append( + _sourceDir, "__main__", "sdformat"); + return true; + } + else + { + // CMake builds set PROJECT_SOURCE_PATH + _sourceDir = PROJECT_SOURCE_PATH; + return true; + } + + return false; + } + + /// \brief Method to retrieve temporary directory for test outputs + /// + /// \param[inout] _tmpDir Full path to the temp directory + /// \return True if directory is set correctly, false otherwise + bool TestTmpPath(std::string &_tmpDir) + { + // Bazel builds set TEST_UNDECLARED_OUTPUTS_DIR + if (env("TEST_UNDECLARED_OUTPUTS_DIR", _tmpDir)) + { + return true; + } + else + { + _tmpDir = sdf::filesystem::append(PROJECT_BINARY_DIR, SDF_TMP_DIR); + return true; + } + } + + /// \brief Method to retrieve temporary home directory for tests + /// + /// This will update the contents of the home directory path variable + /// (HOME on Linux/MacOS, HOMEPATH on Windows) to this newly-set + /// directory + /// This additionally sets the HOME and HOMEPATH environment variables + /// + /// \param[inout] _homeDir Full path to the home directory + /// \return True if directory is set correctly, false otherwise + bool TestSetHomePath(std::string &_homeDir) + { + if (env("TEST_UNDECLARED_OUTPUTS_DIR", _homeDir)) + { + return true; + } + else + { + _homeDir = PROJECT_BINARY_DIR; + // Set both for linux and windows + return setenv("HOME", _homeDir) && setenv("HOMEPATH", _homeDir); + } + } + + /// \brief Retrieve a file from the project source directory + /// \param[in] variable length of arguments relative to the + /// repository source directory + /// \return Full path to requested file + template + std::string SourceFile(Args const &... args) + { + std::string dataDir; + ProjectSourcePath(dataDir); + return sdf::filesystem::append(dataDir, args...); + } + + /// \brief Retrieve a file from the test directory + /// \param[in] variable length of arguments relative to the + /// repository test directory + /// \return Full path to requested test file + template + std::string TestFile(Args const &... args) + { + return SourceFile("test", args...); + } + + } // namespace testing +} // namespace sdf -void unsetenv(const char * name) -{ - std::stringstream sstr; - sstr << *name << '='; - _putenv(sstr.str().c_str()); - return; -} -#endif #endif + From 32540cc273d495b5d28faa7bf666fd88ff7ef7db Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 27 Oct 2021 15:40:26 -0500 Subject: [PATCH 21/60] Backport #721 to sdf9: Make exception for plugins when checking for name uniqueness (#733) * Backport test utilities from sdf10 * Backport Console redirect functionality * Make exception for plugins when checking for name uniqueness (#721) * Replace unavailable error printing API Signed-off-by: Addisu Z. Taddese --- include/sdf/Console.hh | 22 ++++++ include/sdf/Element.hh | 33 ++++++++ src/Console.cc | 24 ++++++ src/Element.cc | 33 +++++++- src/Model.cc | 12 ++- src/World.cc | 12 ++- src/ign_TEST.cc | 17 ++++ src/parser.cc | 3 +- test/integration/model_dom.cc | 57 ++++++++++++++ test/integration/world_dom.cc | 56 ++++++++++++++ test/sdf/model_duplicate_plugins.sdf | 13 ++++ test/sdf/world_duplicate_plugins.sdf | 16 ++++ test/test_utils.hh | 111 +++++++++++++++++++++++++++ 13 files changed, 398 insertions(+), 11 deletions(-) create mode 100644 test/sdf/model_duplicate_plugins.sdf create mode 100644 test/sdf/world_duplicate_plugins.sdf create mode 100644 test/test_utils.hh diff --git a/include/sdf/Console.hh b/include/sdf/Console.hh index 4ff887b51..269d714b4 100644 --- a/include/sdf/Console.hh +++ b/include/sdf/Console.hh @@ -92,6 +92,16 @@ namespace sdf const std::string &_file, unsigned int _line, int _color); + /// \brief Set the stream object. + /// \param[in] _stream Pointer to an output stream. This can be + /// useful for redirecting the output, for example, to a std::stringstream + /// for testing. + public: void SetStream(std::ostream *_stream); + + /// \brief Get the current stream object. + /// \return Pointer to current stream object. + public: std::ostream *GetStream(); + /// \brief The ostream to log to; can be NULL/nullptr. private: std::ostream *stream; }; @@ -128,6 +138,18 @@ namespace sdf const std::string &file, unsigned int line); + /// \brief Get the current message stream object. This can be + /// useful for redirecting the output, for example, to a std::stringstream + /// for testing. + /// \return Mutable reference to current message stream object. + public: ConsoleStream &GetMsgStream(); + + /// \brief Get the current log stream object. This can be + /// useful for redirecting the output, for example, to a std::stringstream + /// for testing. + /// \return Mutable reference to current log stream object. + public: ConsoleStream &GetLogStream(); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh index 810ce7a49..63cd9a2c4 100644 --- a/include/sdf/Element.hh +++ b/include/sdf/Element.hh @@ -350,6 +350,20 @@ namespace sdf /// names. Also return true if no elements of the specified type are found. public: bool HasUniqueChildNames(const std::string &_type = "") const; + /// \brief Checks whether any child elements of the specified element type, + /// except those listed in \p _ignoreElements, have identical name attribute + /// values and returns false if so. + /// \param[in] _type The type of Element to check. If empty, check names + /// of all child elements. + /// \param[in] _ignoreElements A list of child element types to ignore when + /// checking for uniqueness. + /// \return True if all child elements with name attributes of the + /// specified type have unique names, return false if there are duplicated + /// names. Also return true if no elements of the specified type are found. + public: bool HasUniqueChildNames( + const std::string &_type, + const std::vector &_ignoreElements) const; + /// \brief Count the number of child elements of the specified element type /// that have the same name attribute value. /// \param[in] _type The type of Element to check. If empty, count names @@ -360,6 +374,20 @@ namespace sdf public: std::map CountNamedElements(const std::string &_type = "") const; + /// \brief Count the number of child elements of the specified element type + /// that have the same name attribute value with the exception of elements + /// listed in \p _ignoreElements. + /// \param[in] _type The type of Element to check. If empty, count names + /// of all child elements. + /// \param[in] _ignoreElements A list of child element types to ignore when + /// checking for uniqueness. + /// \return Map from Element names to a count of how many times the name + /// occurs. The count should never be 0. If all 2nd values are 1, then + /// there are exclusively unique names. + public: std::map CountNamedElements( + const std::string &_type, + const std::vector &_ignoreElements) const; + /// \brief Return a pointer to the child element with the provided name. /// /// A new child element, with the provided name, is added to this element @@ -467,6 +495,11 @@ namespace sdf /// \return A pointer to the named element if found, nullptr otherwise. public: ElementPtr GetElementImpl(const std::string &_name) const; + /// \brief List of elements to which exceptions are made when checking for + /// name uniqueness. + /// \return List of element types that are allowed to have name collisions. + public: static std::vector NameUniquenessExceptions(); + /// \brief Generate a string (XML) representation of this object. /// \param[in] _prefix arbitrary prefix to put on the string. /// \param[in] _includeDefaultElements flag to include default elements. diff --git a/src/Console.cc b/src/Console.cc index cde264ef5..781491477 100644 --- a/src/Console.cc +++ b/src/Console.cc @@ -105,6 +105,18 @@ void Console::SetQuiet(bool _quiet) g_quiet = _quiet; } +////////////////////////////////////////////////// +sdf::Console::ConsoleStream &Console::GetMsgStream() +{ + return this->dataPtr->msgStream; +} + +////////////////////////////////////////////////// +sdf::Console::ConsoleStream &Console::GetLogStream() +{ + return this->dataPtr->logStream; +} + ////////////////////////////////////////////////// Console::ConsoleStream &Console::ColorMsg(const std::string &lbl, const std::string &file, @@ -157,3 +169,15 @@ void Console::ConsoleStream::Prefix(const std::string &_lbl, _file.substr(index , _file.size() - index)<< ":" << _line << "] "; } } + +////////////////////////////////////////////////// +void Console::ConsoleStream::SetStream(std::ostream *_stream) +{ + this->stream = _stream; +} + +////////////////////////////////////////////////// +std::ostream *Console::ConsoleStream::GetStream() +{ + return this->stream; +} diff --git a/src/Element.cc b/src/Element.cc index 852be9e24..d30d74572 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -764,7 +764,15 @@ std::set Element::GetElementTypeNames() const ///////////////////////////////////////////////// bool Element::HasUniqueChildNames(const std::string &_type) const { - auto namedElementsCount = this->CountNamedElements(_type); + return this->HasUniqueChildNames(_type, {}); +} + +///////////////////////////////////////////////// +bool Element::HasUniqueChildNames( + const std::string &_type, + const std::vector &_ignoreElements) const +{ + auto namedElementsCount = this->CountNamedElements(_type, _ignoreElements); for (auto &iter : namedElementsCount) { if (iter.second > 1) @@ -776,8 +784,16 @@ bool Element::HasUniqueChildNames(const std::string &_type) const } ///////////////////////////////////////////////// -std::map -Element::CountNamedElements(const std::string &_type) const +std::map Element::CountNamedElements( + const std::string &_type) const +{ + return this->CountNamedElements(_type, {}); +} + +///////////////////////////////////////////////// +std::map Element::CountNamedElements( + const std::string &_type, + const std::vector &_ignoreElements) const { std::map result; @@ -793,7 +809,9 @@ Element::CountNamedElements(const std::string &_type) const while (elem) { - if (elem->HasAttribute("name")) + auto ignoreIt = std::find(_ignoreElements.begin(), _ignoreElements.end(), + elem->GetName()); + if (elem->HasAttribute("name") && ignoreIt == _ignoreElements.end()) { // Get("name") returns attribute value if it exists before checking // for the value of a child element , so it's safe to use @@ -1101,3 +1119,10 @@ std::any Element::GetAny(const std::string &_key) const } return result; } + +////////////////////////////////////////////////// +std::vector Element::NameUniquenessExceptions() +{ + // We make exception for "plugin" when checking for name uniqueness. + return {"plugin"}; +} diff --git a/src/Model.cc b/src/Model.cc index c74d1752f..e08ec8e24 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -28,6 +28,7 @@ #include "sdf/Types.hh" #include "FrameSemantics.hh" #include "Utils.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -207,10 +208,15 @@ Errors Model::Load(ElementPtr _sdf) // Load the pose. Ignore the return value since the model pose is optional. loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); - if (!_sdf->HasUniqueChildNames()) + for (const auto &[name, size] : + _sdf->CountNamedElements("", Element::NameUniquenessExceptions())) { - sdfwarn << "Non-unique names detected in XML children of model with name[" - << this->Name() << "].\n"; + if (size > 1) + { + sdfwarn << "Non-unique name[" << name << "] detected " << size + << " times in XML children of model with name[" << this->Name() + << "].\n"; + } } // Set of implicit and explicit frame names in this model for tracking diff --git a/src/World.cc b/src/World.cc index 47342a460..1c1ba5e8e 100644 --- a/src/World.cc +++ b/src/World.cc @@ -28,6 +28,7 @@ #include "sdf/World.hh" #include "FrameSemantics.hh" #include "Utils.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -255,10 +256,15 @@ Errors World::Load(sdf::ElementPtr _sdf) _sdf->Get("magnetic_field", this->dataPtr->magneticField).first; - if (!_sdf->HasUniqueChildNames()) + for (const auto &[name, size] : + _sdf->CountNamedElements("", Element::NameUniquenessExceptions())) { - sdfwarn << "Non-unique names detected in XML children of world with name[" - << this->Name() << "].\n"; + if (size > 1) + { + sdfwarn << "Non-unique name[" << name << "] detected " << size + << " times in XML children of world with name[" << this->Name() + << "].\n"; + } } // Set of implicit and explicit frame names in this model for tracking diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 73935b64a..b4ca3f6aa 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -112,6 +112,14 @@ TEST(check, SDF) EXPECT_NE(output.find("Error: Non-unique names"), std::string::npos) << output; } + // Check an SDF world file is allowed to have duplicate plugin names + { + std::string path = pathBase +"/world_duplicate_plugins.sdf"; + + std::string output = + custom_exec_str(IgnCommand() + " sdf -k " + path + SdfVersion()); + EXPECT_EQ("Valid.\n", output) << output; + } // Check an SDF file with sibling elements of the same type (link) // that have duplicate names. @@ -149,6 +157,15 @@ TEST(check, SDF) << output; } + // Check an SDF model file is allowed to have duplicate plugin names + { + std::string path = pathBase +"/model_duplicate_plugins.sdf"; + + std::string output = + custom_exec_str(IgnCommand() + " sdf -k " + path + SdfVersion()); + EXPECT_EQ("Valid.\n", output) << output; + } + // Check an SDF file with sibling elements of the same type (collision) // that have duplicate names. { diff --git a/src/parser.cc b/src/parser.cc index 97681fda5..8ca9f76d9 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1677,7 +1677,8 @@ bool recursiveSiblingUniqueNames(sdf::ElementPtr _elem) if (!shouldValidateElement(_elem)) return true; - bool result = _elem->HasUniqueChildNames(); + bool result = + _elem->HasUniqueChildNames("", Element::NameUniquenessExceptions()); if (!result) { std::cerr << "Error: Non-unique names detected in " diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 8666cad60..316088348 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -29,6 +29,7 @@ #include "sdf/Types.hh" #include "sdf/World.hh" #include "test_config.h" +#include "test_utils.hh" ////////////////////////////////////////////////// TEST(DOMModel, NotAModel) @@ -374,3 +375,59 @@ TEST(DOMRoot, LoadNestedCanonicalLink) EXPECT_EQ("deep::deeper::deepest::deepest_link", body); } +///////////////////////////////////////////////// +TEST(DOMModel, LoadModelWithDuplicateChildNames) +{ + // Redirect sdfwarn output + 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 + + { + buffer.str(""); + const std::string testFile = + sdf::testing::TestFile("sdf", "model_link_joint_same_name.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + for (const auto &err : errors) + { + std::cout << err << std::endl; + } + + // Check warning message + EXPECT_NE( + std::string::npos, + buffer.str().find("Non-unique name[attachment] detected 2 times in XML " + "children of model with name[link_joint_same_name]")) + << buffer.str(); + } + + // Check that there's an exception for "plugin" elements + { + buffer.str(""); + const std::string testFile = + sdf::testing::TestFile("sdf", "model_duplicate_plugins.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + for (const auto &err : errors) + { + std::cout << err << std::endl; + } + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + } +} diff --git a/test/integration/world_dom.cc b/test/integration/world_dom.cc index 5a529d1a4..cce13c34e 100644 --- a/test/integration/world_dom.cc +++ b/test/integration/world_dom.cc @@ -26,6 +26,7 @@ #include "sdf/Root.hh" #include "sdf/World.hh" #include "test_config.h" +#include "test_utils.hh" ////////////////////////////////////////////////// TEST(DOMWorld, NoName) @@ -216,3 +217,58 @@ TEST(DOMWorld, LoadModelFrameSameName) SemanticPose().Resolve(pose, "ground").empty()); EXPECT_EQ(Pose(0, -2, 3, 0, 0, 0), pose); } + +///////////////////////////////////////////////// +TEST(DOMWorld, LoadWorldWithDuplicateChildNames) +{ + // Redirect sdfwarn output + 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 + + { + buffer.str(""); + const std::string testFile = + sdf::testing::TestFile("sdf", "world_sibling_same_names.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + for (const auto &err : errors) + { + std::cout << err << std::endl; + } + + // Check warning message + EXPECT_NE(std::string::npos, + buffer.str().find("Non-unique name[spot] detected 2 times in XML " + "children of world with name[default]")); + } + + // Check that there's an exception for "plugin" elements + { + buffer.str(""); + const std::string testFile = + sdf::testing::TestFile("sdf", "world_duplicate_plugins.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + for (const auto &err : errors) + { + std::cout << err << std::endl; + } + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + } +} diff --git a/test/sdf/model_duplicate_plugins.sdf b/test/sdf/model_duplicate_plugins.sdf new file mode 100644 index 000000000..a3d2f38e8 --- /dev/null +++ b/test/sdf/model_duplicate_plugins.sdf @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/test/sdf/world_duplicate_plugins.sdf b/test/sdf/world_duplicate_plugins.sdf new file mode 100644 index 000000000..a5c28e7c3 --- /dev/null +++ b/test/sdf/world_duplicate_plugins.sdf @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/test/test_utils.hh b/test/test_utils.hh new file mode 100644 index 000000000..150f70b60 --- /dev/null +++ b/test/test_utils.hh @@ -0,0 +1,111 @@ +/* + * Copyright 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef SDF_TEST_UTILS_HH_ +#define SDF_TEST_UTILS_HH_ + +#include +#include "sdf/Console.hh" + +namespace sdf +{ +namespace testing +{ + +/// \brief Calls a function when going out of scope. +/// Taken from: +/// https://github.com/ros2/rclcpp/blob/master/rclcpp/include/rclcpp/scope_exit.hpp +template +struct ScopeExit +{ + /// \brief Constructor + /// \param[in] _callable Any callable object that does not throw. + explicit ScopeExit(Callable _callable) + : callable(_callable) + { + } + + ~ScopeExit() + { + this->callable(); + } + + private: Callable callable; +}; + +/// \brief A class used for redirecting the output of sdferr, sdfwarn, etc to a +/// more convenient stream object like a std::stringstream for testing purposes. +/// The class reverts to the original stream object when it goes out of scope. +/// +/// Usage example: +/// +/// Redirect console output to a std::stringstream object: +/// +/// ``` +/// std::stringstream buffer; +/// sdf::testing::RedirectConsoleStream redir( +/// sdf::Console::Instance()->GetMsgStream(), &buffer); +/// +/// sdfwarn << "Test message"; +/// +/// ``` +/// `buffer` will now contain "Test message" with additional information, +/// such as the file and line number where sdfwarn was called from. +/// +/// sdfdbg uses a log file as its output, so to redirect that, we can do +/// +/// ``` +/// std::stringstream buffer; +/// sdf::testing::RedirectConsoleStream redir( +/// sdf::Console::Instance()->GetLogStream(), &buffer); +/// +/// sdfdbg << "Test message"; +/// ``` +class RedirectConsoleStream +{ + /// \brief Constructor + /// \param[in] _consoleStream Mutable reference to a console stream. + /// \param[in] _newSink Pointer to any object derived from std::ostream + public: RedirectConsoleStream(sdf::Console::ConsoleStream &_consoleStream, + std::ostream *_newSink) + : consoleStreamRef(_consoleStream) + , oldStream(_consoleStream) + { + this->consoleStreamRef = sdf::Console::ConsoleStream(_newSink); + } + + /// \brief Destructor. Restores the console to the original ConsoleStream + /// object. + public: ~RedirectConsoleStream() + { + this->consoleStreamRef = this->oldStream; + } + + /// \brief Reference to the console stream object. This is usually obtained + /// from the singleton sdf::Console object by calling either + /// sdf::Console::GetMsgStream() or sdf::Console::GetLogStream() + private: sdf::Console::ConsoleStream &consoleStreamRef; + + /// \brief Copy of the original console stream object. This will be used to + /// restore the console stream when this object goes out of scope. + private: sdf::Console::ConsoleStream oldStream; +}; + +} // namespace testing +} // namespace sdf + +#endif + From ceae148bb9628e11154c695e6a24ab94d8619324 Mon Sep 17 00:00:00 2001 From: Nick Lamprianidis Date: Mon, 23 Nov 2020 21:02:01 +0100 Subject: [PATCH 22/60] Add force torque sensor (#393) Signed-off-by: Nick Lamprianidis Co-authored-by: Steve Peters --- include/sdf/CMakeLists.txt | 1 + include/sdf/ForceTorque.hh | 139 +++++++++++++++++++++++++++ include/sdf/Sensor.hh | 12 +++ sdf/1.4/sensor.sdf | 2 +- sdf/1.5/forcetorque.sdf | 2 +- sdf/1.5/sensor.sdf | 2 +- sdf/1.6/forcetorque.sdf | 2 +- sdf/1.6/sensor.sdf | 2 +- sdf/1.7/forcetorque.sdf | 2 +- sdf/1.7/sensor.sdf | 2 +- src/CMakeLists.txt | 2 + src/ForceTorque.cc | 186 +++++++++++++++++++++++++++++++++++++ src/ForceTorque_TEST.cc | 79 ++++++++++++++++ src/Sensor.cc | 28 +++++- 14 files changed, 453 insertions(+), 8 deletions(-) create mode 100644 include/sdf/ForceTorque.hh create mode 100644 src/ForceTorque.cc create mode 100644 src/ForceTorque_TEST.cc diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index 4f0bcdc5f..0ec1a32da 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -15,6 +15,7 @@ set (headers Error.hh Exception.hh Filesystem.hh + ForceTorque.hh Frame.hh Geometry.hh Gui.hh diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh new file mode 100644 index 000000000..2a68aaa31 --- /dev/null +++ b/include/sdf/ForceTorque.hh @@ -0,0 +1,139 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef SDF_FORCE_TORQUE_HH_ +#define SDF_FORCE_TORQUE_HH_ + +#include +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // Forward declare private data class. + class ForceTorquePrivate; + + /// \enum ForceTorqueFrame + /// \brief The set of supported frames of the wrench values. + enum class ForceTorqueFrame : uint8_t + { + /// \brief Invalid frame + INVALID = 0, + + /// \brief Wrench expressed in the orientation of the parent link frame + PARENT = 1, + + /// \brief Wrench expressed in the orientation of the child link frame + CHILD = 2, + + /// \brief Wrench expressed in the orientation of the joint sensor frame + SENSOR = 3 + }; + + /// \enum ForceTorqueMeasureDirection + /// \brief The set of measure directions of the wrench values. + enum class ForceTorqueMeasureDirection : uint8_t + { + /// \brief Invalid frame + INVALID = 0, + + /// \brief Wrench measured as applied by the parent link on the child link + PARENT_TO_CHILD = 1, + + /// \brief Wrench measured as applied by the child link on the parent link + CHILD_TO_PARENT = 2 + }; + + /// \brief ForceTorque contains information about a force torque sensor. + /// This sensor can be attached to a joint. + class SDFORMAT_VISIBLE ForceTorque + { + /// \brief Default constructor + public: ForceTorque(); + + /// \brief Copy constructor + /// \param[in] _ft The force torque sensor to copy. + public: ForceTorque(const ForceTorque &_ft); + + /// \brief Move constructor + /// \param[in] _ft The force torque sensor to move. + public: ForceTorque(ForceTorque &&_ft) noexcept; + + /// \brief Destructor + public: ~ForceTorque(); + + /// \brief Assignment operator. + /// \param[in] _ft The force torque sensor to set values from. + /// \return *this + public: ForceTorque &operator=(const ForceTorque &_ft); + + /// \brief Move assignment operator. + /// \param[in] _ft The force torque sensor to set values from. + /// \return *this + public: ForceTorque &operator=(ForceTorque &&_ft) noexcept; + + /// \brief Load the force torque sensor based on an 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); + + /// \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 + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the frame in which the wrench values are reported. + /// \return The frame of the wrench values. + public: ForceTorqueFrame Frame() const; + + /// \brief Set the frame in which the wrench values are reported. + /// \param[in] _frame The frame of the wrench values. + public: void SetFrame(ForceTorqueFrame _frame) const; + + /// \brief Get the measure direction of the wrench values. + /// \return The measure direction of the wrench values. + public: ForceTorqueMeasureDirection MeasureDirection() const; + + /// \brief Set the measure direction of the wrench values. + /// \param[in] _direction The measure direction of the wrench values. + public: void SetMeasureDirection( + ForceTorqueMeasureDirection _direction) const; + + /// \brief Return true if both force torque objects contain the same values. + /// \param[_in] _ft Force torque value to compare. + /// \returen True if 'this' == _ft. + public: bool operator==(const ForceTorque &_ft) const; + + /// \brief Return true this force torque object does not contain the same + /// values as the passed-in parameter. + /// \param[_in] _ft Force torque value to compare. + /// \returen True if 'this' != _ft. + public: bool operator!=(const ForceTorque &_ft) const; + + /// \brief Private data pointer. + private: ForceTorquePrivate *dataPtr; + }; + } +} + +#endif diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 712b9b29a..d66320d04 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -35,6 +35,7 @@ namespace sdf class AirPressure; class Altimeter; class Camera; + class ForceTorque; class Imu; class Lidar; class Magnetometer; @@ -331,6 +332,17 @@ namespace sdf /// \sa SensorType Type() const public: const Camera *CameraSensor() const; + /// \brief Set the force torque sensor. + /// \param[in] _ft The force torque sensor. + public: void SetForceTorqueSensor(const ForceTorque &_ft); + + /// \brief Get a pointer to a force torque sensor, or nullptr if the sensor + /// does not contain a force torque sensor. + /// \return Pointer to the force torque sensor, or nullptr if the sensor + /// is not a force torque sensor. + /// \sa SensorType Type() const + public: const ForceTorque *ForceTorqueSensor() const; + /// \brief Set the NAVSAT sensor. /// \param[in] _navsat The NAVSAT sensor. public: void SetNavSatSensor(const NavSat &_navsat); diff --git a/sdf/1.4/sensor.sdf b/sdf/1.4/sensor.sdf index df4749333..1d815338e 100644 --- a/sdf/1.4/sensor.sdf +++ b/sdf/1.4/sensor.sdf @@ -33,6 +33,7 @@ + @@ -40,6 +41,5 @@ - diff --git a/sdf/1.5/forcetorque.sdf b/sdf/1.5/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.5/forcetorque.sdf +++ b/sdf/1.5/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.5/sensor.sdf b/sdf/1.5/sensor.sdf index 5891cfa7f..e58ae79b3 100644 --- a/sdf/1.5/sensor.sdf +++ b/sdf/1.5/sensor.sdf @@ -49,6 +49,7 @@ + @@ -58,6 +59,5 @@ - diff --git a/sdf/1.6/forcetorque.sdf b/sdf/1.6/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.6/forcetorque.sdf +++ b/sdf/1.6/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.6/sensor.sdf b/sdf/1.6/sensor.sdf index 006b946bc..f3272df1b 100644 --- a/sdf/1.6/sensor.sdf +++ b/sdf/1.6/sensor.sdf @@ -57,6 +57,7 @@ + @@ -67,6 +68,5 @@ - diff --git a/sdf/1.7/forcetorque.sdf b/sdf/1.7/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.7/forcetorque.sdf +++ b/sdf/1.7/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.7/sensor.sdf b/sdf/1.7/sensor.sdf index 58588b28d..a2eaf689c 100644 --- a/sdf/1.7/sensor.sdf +++ b/sdf/1.7/sensor.sdf @@ -61,6 +61,7 @@ + @@ -72,6 +73,5 @@ - diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 08362bf88..a803bf88b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -29,6 +29,7 @@ set (sources Frame.cc FrameSemantics.cc Filesystem.cc + ForceTorque.cc Geometry.cc Gui.cc Heightmap.cc @@ -110,6 +111,7 @@ if (BUILD_SDF_TEST) Exception_TEST.cc Frame_TEST.cc Filesystem_TEST.cc + ForceTorque_TEST.cc Geometry_TEST.cc Gui_TEST.cc Heightmap_TEST.cc diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc new file mode 100644 index 000000000..8a467d5c4 --- /dev/null +++ b/src/ForceTorque.cc @@ -0,0 +1,186 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include +#include "sdf/ForceTorque.hh" + +using namespace sdf; + +/// \brief Private force torque data. +class sdf::ForceTorquePrivate +{ + /// \brief Name of the reference frame for the wrench values. + public: ForceTorqueFrame frame = ForceTorqueFrame::CHILD; + + /// \brief The direction of the measured wrench values. + public: ForceTorqueMeasureDirection measure_direction = + ForceTorqueMeasureDirection::CHILD_TO_PARENT; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +////////////////////////////////////////////////// +ForceTorque::ForceTorque() + : dataPtr(new ForceTorquePrivate) +{ +} + +////////////////////////////////////////////////// +ForceTorque::~ForceTorque() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +ForceTorque::ForceTorque(const ForceTorque &_ft) + : dataPtr(new ForceTorquePrivate(*_ft.dataPtr)) +{ +} + +////////////////////////////////////////////////// +ForceTorque::ForceTorque(ForceTorque &&_ft) noexcept + : dataPtr(std::exchange(_ft.dataPtr, nullptr)) +{ +} + +////////////////////////////////////////////////// +ForceTorque &ForceTorque::operator=(const ForceTorque &_ft) +{ + return *this = ForceTorque(_ft); +} + +////////////////////////////////////////////////// +ForceTorque &ForceTorque::operator=(ForceTorque &&_ft) noexcept +{ + std::swap(this->dataPtr, _ft.dataPtr); + return *this; +} + +////////////////////////////////////////////////// +Errors ForceTorque::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that the provided SDF element is a element. + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "force_torque") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a force torque sensor, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("frame")) + { + std::string frame = _sdf->Get("frame", "child").first; + + if (frame == "parent") + { + this->dataPtr->frame = ForceTorqueFrame::PARENT; + } + else if (frame == "child") + { + this->dataPtr->frame = ForceTorqueFrame::CHILD; + } + else if (frame == "sensor") + { + this->dataPtr->frame = ForceTorqueFrame::SENSOR; + } + else + { + this->dataPtr->frame = ForceTorqueFrame::INVALID; + errors.push_back({ErrorCode::ELEMENT_INVALID, + "ForceTorque element 'frame' is invalid with a value of [" + frame + + "]. Refer to the SDF documentation for the list of valid frames"}); + } + } + + if (_sdf->HasElement("measure_direction")) + { + std::string direction = + _sdf->Get("measure_direction", "child_to_parent").first; + + if (direction == "parent_to_child") + { + this->dataPtr->measure_direction = + ForceTorqueMeasureDirection::PARENT_TO_CHILD; + } + else if (direction == "child_to_parent") + { + this->dataPtr->measure_direction = + ForceTorqueMeasureDirection::CHILD_TO_PARENT; + } + else + { + this->dataPtr->measure_direction = ForceTorqueMeasureDirection::INVALID; + errors.push_back({ErrorCode::ELEMENT_INVALID, + "ForceTorque element 'measure_direction' is invalid with a value " + "of [" + direction + "]. Refer to the SDF documentation for the " + "list of valid frames"}); + } + } + + return errors; +} + +////////////////////////////////////////////////// +sdf::ElementPtr ForceTorque::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +bool ForceTorque::operator!=(const ForceTorque &_ft) const +{ + return !(*this == _ft); +} + +////////////////////////////////////////////////// +bool ForceTorque::operator==(const ForceTorque &_ft) const +{ + return this->dataPtr->frame == _ft.dataPtr->frame && + this->dataPtr->measure_direction == _ft.dataPtr->measure_direction; +} + +////////////////////////////////////////////////// +ForceTorqueFrame ForceTorque::Frame() const +{ + return this->dataPtr->frame; +} + +////////////////////////////////////////////////// +void ForceTorque::SetFrame(ForceTorqueFrame _frame) const +{ + this->dataPtr->frame = _frame; +} + +////////////////////////////////////////////////// +ForceTorqueMeasureDirection ForceTorque::MeasureDirection() const +{ + return this->dataPtr->measure_direction; +} + +////////////////////////////////////////////////// +void ForceTorque::SetMeasureDirection( + ForceTorqueMeasureDirection _direction) const +{ + this->dataPtr->measure_direction = _direction; +} diff --git a/src/ForceTorque_TEST.cc b/src/ForceTorque_TEST.cc new file mode 100644 index 000000000..57bff24f8 --- /dev/null +++ b/src/ForceTorque_TEST.cc @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include "sdf/ForceTorque.hh" + +///////////////////////////////////////////////// +TEST(DOMForceTorque, Construction) +{ + sdf::ForceTorque ft; + + EXPECT_EQ(ft.Frame(), sdf::ForceTorqueFrame::CHILD); + ft.SetFrame(sdf::ForceTorqueFrame::PARENT); + EXPECT_EQ(ft.Frame(), sdf::ForceTorqueFrame::PARENT); + + EXPECT_EQ(ft.MeasureDirection(), + sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT); + ft.SetMeasureDirection(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD); + EXPECT_EQ(ft.MeasureDirection(), + sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD); + + // Copy constructor + sdf::ForceTorque ft2(ft); + EXPECT_EQ(ft2, ft); + + // Copy operator + sdf::ForceTorque ft3; + ft3 = ft; + EXPECT_EQ(ft3, ft); + + // Move constructor + sdf::ForceTorque ft4(std::move(ft)); + EXPECT_EQ(ft4, ft2); + ft = ft4; + EXPECT_EQ(ft, ft2); + + // Move operator + sdf::ForceTorque ft5; + ft5 = std::move(ft2); + EXPECT_EQ(ft5, ft3); + ft2 = ft5; + EXPECT_EQ(ft2, ft3); + + // Inequality + sdf::ForceTorque ft6; + EXPECT_NE(ft6, ft3); +} + +///////////////////////////////////////////////// +TEST(DOMForceTorque, Load) +{ + sdf::ElementPtr sdf(std::make_shared()); + + sdf::ForceTorque ft; + sdf::Errors errors = ft.Load(sdf); + EXPECT_FALSE(errors.empty()); + EXPECT_TRUE(errors[0].Message().find("is not a ") + != std::string::npos) << errors[0].Message(); + + EXPECT_NE(nullptr, ft.Element()); + EXPECT_EQ(sdf.get(), ft.Element().get()); + + // The ForceTorque::Load function is tested more thouroughly in the + // link_dom.cc integration test. +} diff --git a/src/Sensor.cc b/src/Sensor.cc index 92a911487..c6a4036f5 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -22,6 +22,7 @@ #include "sdf/Altimeter.hh" #include "sdf/Camera.hh" #include "sdf/Error.hh" +#include "sdf/ForceTorque.hh" #include "sdf/NavSat.hh" #include "sdf/Imu.hh" #include "sdf/Magnetometer.hh" @@ -99,6 +100,11 @@ class sdf::SensorPrivate { this->camera = std::make_unique(*_sensor.camera); } + if (_sensor.forceTorque) + { + this->forceTorque = std::make_unique( + *_sensor.forceTorque); + } if (_sensor.imu) { this->imu = std::make_unique(*_sensor.imu); @@ -156,6 +162,9 @@ class sdf::SensorPrivate /// \brief Pointer to a camera. public: std::unique_ptr camera; + /// \brief Pointer to a force torque sensor. + public: std::unique_ptr forceTorque; + /// \brief Pointer to an IMU. public: std::unique_ptr imu; @@ -233,6 +242,8 @@ bool Sensor::operator==(const Sensor &_sensor) const return *(this->dataPtr->magnetometer) == *(_sensor.dataPtr->magnetometer); case SensorType::AIR_PRESSURE: return *(this->dataPtr->airPressure) == *(_sensor.dataPtr->airPressure); + case SensorType::FORCE_TORQUE: + return *(this->dataPtr->forceTorque) == *(_sensor.dataPtr->forceTorque); case SensorType::IMU: return *(this->dataPtr->imu) == *(_sensor.dataPtr->imu); case SensorType::NAVSAT: @@ -357,6 +368,10 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "force_torque") { this->dataPtr->type = SensorType::FORCE_TORQUE; + this->dataPtr->forceTorque.reset(new ForceTorque()); + Errors err = this->dataPtr->forceTorque->Load( + _sdf->GetElement("force_torque")); + errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "navsat" || type == "gps") { @@ -376,7 +391,6 @@ Errors Sensor::Load(ElementPtr _sdf) } else if (type == "imu") { - this->dataPtr->type = SensorType::IMU; this->dataPtr->type = SensorType::IMU; this->dataPtr->imu.reset(new Imu()); Errors err = this->dataPtr->imu->Load(_sdf->GetElement("imu")); @@ -660,6 +674,18 @@ const Camera *Sensor::CameraSensor() const return this->dataPtr->camera.get(); } +///////////////////////////////////////////////// +void Sensor::SetForceTorqueSensor(const ForceTorque &_ft) +{ + this->dataPtr->forceTorque = std::make_unique(_ft); +} + +///////////////////////////////////////////////// +const ForceTorque *Sensor::ForceTorqueSensor() const +{ + return this->dataPtr->forceTorque.get(); +} + ///////////////////////////////////////////////// void Sensor::SetNavSatSensor(const NavSat &_gps) { From 798aad22a08fa1d0d6991a8ab374978b55596973 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 24 Mar 2021 17:21:07 -0500 Subject: [PATCH 23/60] Add Joint DOM API to access joint sensors (#517) * Add Joint DOM API to access joint sensors Signed-off-by: Addisu Z. Taddese * Add missing file Signed-off-by: Addisu Z. Taddese * modified Joint::SensorNameExists Signed-off-by: Jenn Nguyen Co-authored-by: Jenn Nguyen --- include/sdf/Joint.hh | 24 +++++++++++++++++++ src/Joint.cc | 41 +++++++++++++++++++++++++++++++ src/Joint_TEST.cc | 7 ++++++ test/integration/joint_dom.cc | 45 +++++++++++++++++++++++++++++++++++ test/sdf/joint_sensors.sdf | 18 ++++++++++++++ 5 files changed, 135 insertions(+) create mode 100644 test/sdf/joint_sensors.sdf diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index a9fd3bb3f..4a97d12e8 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -36,6 +36,7 @@ namespace sdf class JointAxis; class JointPrivate; struct PoseRelativeToGraph; + class Sensor; /// \enum JointType /// \brief The set of joint types. INVALID indicates that joint type has @@ -240,6 +241,29 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; + /// \brief Get the number of sensors. + /// \return Number of sensors contained in this Joint object. + public: uint64_t SensorCount() const; + + /// \brief Get a sensor based on an index. + /// \param[in] _index Index of the sensor. The index should be in the + /// range [0..SensorCount()). + /// \return Pointer to the sensor. Nullptr if the index does not exist. + /// \sa uint64_t SensorCount() const + public: const Sensor *SensorByIndex(const uint64_t _index) const; + + /// \brief Get whether a sensor name exists. + /// \param[in] _name Name of the sensor to check. + /// \return True if there exists a sensor with the given name. + public: bool SensorNameExists(const std::string &_name) const; + + /// \brief Get a sensor based on a name. + /// \param[in] _name Name of the sensor. + /// \return Pointer to the sensor. Nullptr if a sensor with the given name + /// does not exist. + /// \sa bool SensorNameExists(const std::string &_name) const + public: const Sensor *SensorByName(const std::string &_name) const; + /// \brief Give a weak pointer to the PoseRelativeToGraph to be used /// for resolving poses. This is private and is intended to be called by /// Model::Load. diff --git a/src/Joint.cc b/src/Joint.cc index 78731090d..f33e8e90d 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -23,6 +23,7 @@ #include "sdf/Error.hh" #include "sdf/Joint.hh" #include "sdf/JointAxis.hh" +#include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "Utils.hh" @@ -74,6 +75,9 @@ class sdf::JointPrivate /// \brief Weak pointer to model's Pose Relative-To Graph. public: std::weak_ptr poseRelativeToGraph; + + /// \brief The sensors specified in this joint. + public: std::vector sensors; }; ///////////////////////////////////////////////// @@ -265,6 +269,10 @@ Errors Joint::Load(ElementPtr _sdf) "A joint type is required, but is not set."}); } + // Load all the sensors. + Errors sensorLoadErrors = loadUniqueRepeated(_sdf, "sensor", + this->dataPtr->sensors); + errors.insert(errors.end(), sensorLoadErrors.begin(), sensorLoadErrors.end()); return errors; } @@ -420,3 +428,36 @@ sdf::ElementPtr Joint::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +uint64_t Joint::SensorCount() const +{ + return this->dataPtr->sensors.size(); +} + +///////////////////////////////////////////////// +const Sensor *Joint::SensorByIndex(const uint64_t _index) const +{ + if (_index < this->dataPtr->sensors.size()) + return &this->dataPtr->sensors[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +bool Joint::SensorNameExists(const std::string &_name) const +{ + return nullptr != this->SensorByName(_name); +} + +///////////////////////////////////////////////// +const Sensor *Joint::SensorByName(const std::string &_name) const +{ + for (auto const &s : this->dataPtr->sensors) + { + if (s.Name() == _name) + { + return &s; + } + } + return nullptr; +} diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index 1f1fc6100..fae9c7ad4 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -98,6 +98,13 @@ TEST(DOMJoint, Construction) const double threadPitch = 0.1; joint.SetThreadPitch(threadPitch); EXPECT_DOUBLE_EQ(threadPitch, joint.ThreadPitch()); + + EXPECT_EQ(0u, joint.SensorCount()); + EXPECT_EQ(nullptr, joint.SensorByIndex(0)); + EXPECT_EQ(nullptr, joint.SensorByIndex(1)); + EXPECT_EQ(nullptr, joint.SensorByName("empty")); + EXPECT_FALSE(joint.SensorNameExists("")); + EXPECT_FALSE(joint.SensorNameExists("default")); } ///////////////////////////////////////////////// diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 379b644b1..bc8ac2868 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -20,12 +20,14 @@ #include "sdf/Element.hh" #include "sdf/Filesystem.hh" +#include "sdf/ForceTorque.hh" #include "sdf/Joint.hh" #include "sdf/JointAxis.hh" #include "sdf/Link.hh" #include "sdf/Model.hh" #include "sdf/Root.hh" #include "sdf/SDFImpl.hh" +#include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "sdf/parser.hh" #include "test_config.h" @@ -564,3 +566,46 @@ TEST(DOMJoint, LoadURDFJointPoseRelativeTo) model->JointByName("joint12")->Axis()->ResolveXyz(vec3, "joint12").empty()); EXPECT_EQ(Vector3(0, 1.0, 0), vec3); } + +////////////////////////////////////////////////// +TEST(DOMJoint, Sensors) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "joint_sensors.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + for (const auto &err : errors) + { + std::cout << err << std::endl; + } + + // Get the first model + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("model", model->Name()); + + // Get the first joint + const sdf::Joint *joint = model->JointByIndex(0); + ASSERT_NE(nullptr, joint); + EXPECT_EQ("joint", joint->Name()); + EXPECT_EQ(1u, joint->SensorCount()); + + ignition::math::Pose3d pose; + + // Get the force_torque sensor + const sdf::Sensor *forceTorqueSensor = + joint->SensorByName("force_torque_sensor"); + ASSERT_NE(nullptr, forceTorqueSensor); + EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name()); + EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + forceTorqueSensor->RawPose()); + auto forceTorqueSensorConfig = forceTorqueSensor->ForceTorqueSensor(); + ASSERT_NE(nullptr, forceTorqueSensorConfig); + EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorConfig->Frame()); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, + forceTorqueSensorConfig->MeasureDirection()); +} diff --git a/test/sdf/joint_sensors.sdf b/test/sdf/joint_sensors.sdf new file mode 100644 index 000000000..16b63c89b --- /dev/null +++ b/test/sdf/joint_sensors.sdf @@ -0,0 +1,18 @@ + + + + + + + link1 + link2 + + 10 11 12 0 0 0 + + parent + parent_to_child + + + + + From 209eba93f5f51e6bd4e981465bf468d4a93645a6 Mon Sep 17 00:00:00 2001 From: Devansh Chawla Date: Tue, 21 Sep 2021 15:34:59 -0400 Subject: [PATCH 24/60] Added Force Torque Noise functions + Unit tests (#669) * Added Force Torque Noise functions + Unit tests Signed-off-by: Devansh Co-authored-by: Michael Carroll Co-authored-by: Steve Peters --- include/sdf/ForceTorque.hh | 48 +++++++++++++ sdf/1.7/forcetorque.sdf | 34 +++++++++ src/ForceTorque.cc | 127 +++++++++++++++++++++++++++++++++- src/ForceTorque_TEST.cc | 32 +++++++++ test/integration/joint_dom.cc | 23 ++++-- test/sdf/joint_sensors.sdf | 40 +++++++++++ 6 files changed, 299 insertions(+), 5 deletions(-) diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh index 2a68aaa31..eed921726 100644 --- a/include/sdf/ForceTorque.hh +++ b/include/sdf/ForceTorque.hh @@ -102,6 +102,54 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Get the force noise values in the measurement frame X-axis. + /// \return Noise values for the X-axis force. + public: const Noise &ForceXNoise() const; + + /// \brief Set the force noise values in the measurement frame X-axis. + /// \param[in] _noise Noise values for the X-axis force. + public: void SetForceXNoise(const Noise &_noise); + + /// \brief Get the force noise values in the measurement frame Y-axis. + /// \return Noise values for the Y-axis force. + public: const Noise &ForceYNoise() const; + + /// \brief Set the force noise values in the measurement frame Y-axis. + /// \param[in] _noise Noise values for the Y-axis force. + public: void SetForceYNoise(const Noise &_noise); + + /// \brief Get the force noise values in the measurement frame Z-axis. + /// \return Noise values for the Z-axis force. + public: const Noise &ForceZNoise() const; + + /// \brief Set the force noise values in the measurement frame Z-axis. + /// \param[in] _noise Noise values for the Z-axis force. + public: void SetForceZNoise(const Noise &_noise); + + /// \brief Get the torque noise values in the measurement frame X-axis. + /// \return Noise values for the X-axis torque. + public: const Noise &TorqueXNoise() const; + + /// \brief Set the torque noise values in the measurement frame X-axis. + /// \param[in] _noise Noise values for the X-axis torque. + public: void SetTorqueXNoise(const Noise &_noise); + + /// \brief Get the torque noise values in the measurement frame Y-axis. + /// \return Noise values for the Y-axis torque. + public: const Noise &TorqueYNoise() const; + + /// \brief Set the torque noise values in the measurement frame Y-axis. + /// \param[in] _noise Noise values for the Y-axis torque. + public: void SetTorqueYNoise(const Noise &_noise); + + /// \brief Get the torque noise values in the measurement frame Z-axis. + /// \return Noise values for the Z-axis torque. + public: const Noise &TorqueZNoise() const; + + /// \brief Set the torque noise values in the measurement frame Z-axis. + /// \param[in] _noise Noise values for the Z-axis torque. + public: void SetTorqueZNoise(const Noise &_noise); + /// \brief Get the frame in which the wrench values are reported. /// \return The frame of the wrench values. public: ForceTorqueFrame Frame() const; diff --git a/sdf/1.7/forcetorque.sdf b/sdf/1.7/forcetorque.sdf index 18e5b8f71..115d34c6f 100644 --- a/sdf/1.7/forcetorque.sdf +++ b/sdf/1.7/forcetorque.sdf @@ -17,4 +17,38 @@ "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. + + + These elements are specific to measurement-frame force, + which is expressed in Newtons + + Force along the X axis + + + + Force along the Y axis + + + + Force along the Z axis + + + + + + These elements are specific to measurement-frame torque, + which is expressed in Newton-meters + + Torque about the X axis + + + + Force about the Y axis + + + + Torque about the Z axis + + + diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc index 8a467d5c4..aab5b9cca 100644 --- a/src/ForceTorque.cc +++ b/src/ForceTorque.cc @@ -22,6 +22,30 @@ using namespace sdf; /// \brief Private force torque data. class sdf::ForceTorquePrivate { + /// \brief Noise values related to the body-frame force on the + /// X-axis. + public: Noise forceXNoise; + + /// \brief Noise values related to the body-frame force on the + /// Y-axis. + public: Noise forceYNoise; + + /// \brief Noise values related to the body-frame force on the + /// Z-axis. + public: Noise forceZNoise; + + /// \brief Noise values related to the body-frame torque on the + /// X-axis. + public: Noise torqueXNoise; + + /// \brief Noise values related to the body-frame torque on the + /// Y-axis. + public: Noise torqueYNoise; + + /// \brief Noise values related to the body-frame torque on the + /// Z-axis. + public: Noise torqueZNoise; + /// \brief Name of the reference frame for the wrench values. public: ForceTorqueFrame frame = ForceTorqueFrame::CHILD; @@ -138,6 +162,29 @@ Errors ForceTorque::Load(ElementPtr _sdf) } } + auto loadAxisNoise = [&errors](sdf::ElementPtr _parent, + const std::string _groupLabel, + const std::string _axisLabel, + sdf::Noise& _noise) + { + if (_parent->HasElement(_groupLabel) && + _parent->GetElement(_groupLabel)->HasElement(_axisLabel)) + { + auto axis = _parent->GetElement(_groupLabel)->GetElement(_axisLabel); + sdf::Errors noiseErrors = _noise.Load(axis->GetElement("noise")); + errors.insert(errors.end(), noiseErrors.begin(), noiseErrors.end()); + return true; + } + return false; + }; + + loadAxisNoise(_sdf, "force", "x", this->dataPtr->forceXNoise); + loadAxisNoise(_sdf, "force", "y", this->dataPtr->forceYNoise); + loadAxisNoise(_sdf, "force", "z", this->dataPtr->forceZNoise); + loadAxisNoise(_sdf, "torque", "x", this->dataPtr->torqueXNoise); + loadAxisNoise(_sdf, "torque", "y", this->dataPtr->torqueYNoise); + loadAxisNoise(_sdf, "torque", "z", this->dataPtr->torqueZNoise); + return errors; } @@ -157,7 +204,85 @@ bool ForceTorque::operator!=(const ForceTorque &_ft) const bool ForceTorque::operator==(const ForceTorque &_ft) const { return this->dataPtr->frame == _ft.dataPtr->frame && - this->dataPtr->measure_direction == _ft.dataPtr->measure_direction; + this->dataPtr->measure_direction == _ft.dataPtr->measure_direction && + this->dataPtr->forceXNoise == _ft.dataPtr->forceXNoise && + this->dataPtr->forceYNoise == _ft.dataPtr->forceYNoise && + this->dataPtr->forceZNoise == _ft.dataPtr->forceZNoise && + this->dataPtr->torqueXNoise == _ft.dataPtr->torqueXNoise && + this->dataPtr->torqueYNoise == _ft.dataPtr->torqueYNoise && + this->dataPtr->torqueZNoise == _ft.dataPtr->torqueZNoise; +} + +////////////////////////////////////////////////// +const Noise &ForceTorque::ForceXNoise() const +{ + return this->dataPtr->forceXNoise; +} + +////////////////////////////////////////////////// +void ForceTorque::SetForceXNoise(const Noise &_noise) +{ + this->dataPtr->forceXNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &ForceTorque::ForceYNoise() const +{ + return this->dataPtr->forceYNoise; +} + +////////////////////////////////////////////////// +void ForceTorque::SetForceYNoise(const Noise &_noise) +{ + this->dataPtr->forceYNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &ForceTorque::ForceZNoise() const +{ + return this->dataPtr->forceZNoise; +} + +////////////////////////////////////////////////// +void ForceTorque::SetForceZNoise(const Noise &_noise) +{ + this->dataPtr->forceZNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &ForceTorque::TorqueXNoise() const +{ + return this->dataPtr->torqueXNoise; +} + +////////////////////////////////////////////////// +void ForceTorque::SetTorqueXNoise(const Noise &_noise) +{ + this->dataPtr->torqueXNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &ForceTorque::TorqueYNoise() const +{ + return this->dataPtr->torqueYNoise; +} + +////////////////////////////////////////////////// +void ForceTorque::SetTorqueYNoise(const Noise &_noise) +{ + this->dataPtr->torqueYNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &ForceTorque::TorqueZNoise() const +{ + return this->dataPtr->torqueZNoise; +} + +////////////////////////////////////////////////// +void ForceTorque::SetTorqueZNoise(const Noise &_noise) +{ + this->dataPtr->torqueZNoise = _noise; } ////////////////////////////////////////////////// diff --git a/src/ForceTorque_TEST.cc b/src/ForceTorque_TEST.cc index 57bff24f8..6acba8514 100644 --- a/src/ForceTorque_TEST.cc +++ b/src/ForceTorque_TEST.cc @@ -22,6 +22,38 @@ TEST(DOMForceTorque, Construction) { sdf::ForceTorque ft; + sdf::Noise defaultNoise, noise; + + noise.SetType(sdf::NoiseType::GAUSSIAN); + noise.SetMean(1.2); + noise.SetStdDev(2.3); + noise.SetBiasMean(4.5); + noise.SetBiasStdDev(6.7); + noise.SetPrecision(8.9); + + EXPECT_EQ(defaultNoise, ft.ForceXNoise()); + ft.SetForceXNoise(noise); + EXPECT_EQ(noise, ft.ForceXNoise()); + + EXPECT_EQ(defaultNoise, ft.ForceYNoise()); + ft.SetForceYNoise(noise); + EXPECT_EQ(noise, ft.ForceYNoise()); + + EXPECT_EQ(defaultNoise, ft.ForceZNoise()); + ft.SetForceZNoise(noise); + EXPECT_EQ(noise, ft.ForceZNoise()); + + EXPECT_EQ(defaultNoise, ft.TorqueXNoise()); + ft.SetTorqueXNoise(noise); + EXPECT_EQ(noise, ft.TorqueXNoise()); + + EXPECT_EQ(defaultNoise, ft.TorqueYNoise()); + ft.SetTorqueYNoise(noise); + EXPECT_EQ(noise, ft.TorqueYNoise()); + + EXPECT_EQ(defaultNoise, ft.TorqueZNoise()); + ft.SetTorqueZNoise(noise); + EXPECT_EQ(noise, ft.TorqueZNoise()); EXPECT_EQ(ft.Frame(), sdf::ForceTorqueFrame::CHILD); ft.SetFrame(sdf::ForceTorqueFrame::PARENT); diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index bc8ac2868..a9290a0c9 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -603,9 +603,24 @@ TEST(DOMJoint, Sensors) EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type()); EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), forceTorqueSensor->RawPose()); - auto forceTorqueSensorConfig = forceTorqueSensor->ForceTorqueSensor(); - ASSERT_NE(nullptr, forceTorqueSensorConfig); - EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorConfig->Frame()); + auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor(); + ASSERT_NE(nullptr, forceTorqueSensorObj); + EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorObj->Frame()); EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, - forceTorqueSensorConfig->MeasureDirection()); + forceTorqueSensorObj->MeasureDirection()); + + EXPECT_DOUBLE_EQ(0.0, forceTorqueSensorObj->ForceXNoise().Mean()); + EXPECT_DOUBLE_EQ(0.1, forceTorqueSensorObj->ForceXNoise().StdDev()); + EXPECT_DOUBLE_EQ(1.0, forceTorqueSensorObj->ForceYNoise().Mean()); + EXPECT_DOUBLE_EQ(1.1, forceTorqueSensorObj->ForceYNoise().StdDev()); + EXPECT_DOUBLE_EQ(2.0, forceTorqueSensorObj->ForceZNoise().Mean()); + EXPECT_DOUBLE_EQ(2.1, forceTorqueSensorObj->ForceZNoise().StdDev()); + + EXPECT_DOUBLE_EQ(3.0, forceTorqueSensorObj->TorqueXNoise().Mean()); + EXPECT_DOUBLE_EQ(3.1, forceTorqueSensorObj->TorqueXNoise().StdDev()); + EXPECT_DOUBLE_EQ(4.0, forceTorqueSensorObj->TorqueYNoise().Mean()); + EXPECT_DOUBLE_EQ(4.1, forceTorqueSensorObj->TorqueYNoise().StdDev()); + EXPECT_DOUBLE_EQ(5.0, forceTorqueSensorObj->TorqueZNoise().Mean()); + EXPECT_DOUBLE_EQ(5.1, forceTorqueSensorObj->TorqueZNoise().StdDev()); + } diff --git a/test/sdf/joint_sensors.sdf b/test/sdf/joint_sensors.sdf index 16b63c89b..9707c9563 100644 --- a/test/sdf/joint_sensors.sdf +++ b/test/sdf/joint_sensors.sdf @@ -11,6 +11,46 @@ parent parent_to_child + + + + 0 + 0.1 + + + + + 1 + 1.1 + + + + + 2 + 2.1 + + + + + + + 3 + 3.1 + + + + + 4 + 4.1 + + + + + 5 + 5.1 + + + From b240384469eb0136867381d692e1839457e4ce37 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 2 Nov 2021 13:13:46 -0700 Subject: [PATCH 25/60] Support accessing mutable sensor types (#737) * Support accessing mutable sensor types Signed-off-by: Nate Koenig * Added simple test Signed-off-by: Nate Koenig * Added more tests Signed-off-by: Nate Koenig * Added asserts Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/Sensor.hh | 56 +++++++++++++++ src/Sensor.cc | 48 +++++++++++++ src/Sensor_TEST.cc | 164 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 268 insertions(+) diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index f8ac4adcd..ef3ddbd08 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -253,6 +253,13 @@ namespace sdf /// \sa SensorType Type() const public: const Magnetometer *MagnetometerSensor() const; + /// \brief Get a mutable magnetometer sensor, or nullptr if this sensor type + /// is not a Magnetometer. + /// \return Pointer to the Magnetometer sensor, or nullptr if this + /// Sensor is not a Magnetometer. + /// \sa SensorType Type() const + public: Magnetometer *MagnetometerSensor(); + /// \brief Set the magnetometer sensor. /// \param[in] _mag The magnetometer sensor. public: void SetMagnetometerSensor(const Magnetometer &_mag); @@ -264,6 +271,13 @@ namespace sdf /// \sa SensorType Type() const public: const Altimeter *AltimeterSensor() const; + /// \brief Get a mutable altimeter sensor, or nullptr if this sensor type + /// is not an Altimeter. + /// \return Pointer to the Altimeter sensor, or nullptr if this + /// Sensor is not a Altimeter. + /// \sa SensorType Type() const + public: Altimeter *AltimeterSensor(); + /// \brief Set the altimeter sensor. /// \param[in] _alt The altimeter sensor. public: void SetAltimeterSensor(const Altimeter &_alt); @@ -275,6 +289,13 @@ namespace sdf /// \sa SensorType Type() const public: const AirPressure *AirPressureSensor() const; + /// \brief Get a mutable air pressure sensor, or nullptr if this sensor type + /// is not an AirPressure sensor. + /// \return Pointer to the AirPressure sensor, or nullptr if this + /// Sensor is not a AirPressure sensor. + /// \sa SensorType Type() const + public: AirPressure *AirPressureSensor(); + /// \brief Set the air pressure sensor. /// \param[in] _air The air pressure sensor. public: void SetAirPressureSensor(const AirPressure &_air); @@ -290,6 +311,13 @@ namespace sdf /// \sa SensorType Type() const public: const Camera *CameraSensor() const; + /// \brief Get a mutable camera sensor, or nullptr if the + /// sensor does not contain a camera sensor. + /// \return Pointer to the sensor's camera, or nullptr if the sensor + /// is not a camera. + /// \sa SensorType Type() const + public: Camera *CameraSensor(); + /// \brief Set the NAVSAT sensor. /// \param[in] _navsat The NAVSAT sensor. public: void SetNavSatSensor(const NavSat &_navsat); @@ -301,6 +329,13 @@ namespace sdf /// \sa SensorType Type() const public: const NavSat *NavSatSensor() const; + /// \brief Get a mutable NAVSAT sensor, or nullptr if the sensor + /// does not contain an NAVSAT sensor. + /// \return Pointer to the sensor's NAVSAT, or nullptr if the sensor + /// is not an NAVSAT. + /// \sa SensorType Type() const + public: NavSat *NavSatSensor(); + /// \brief Set the force torque sensor. /// \param[in] _ft The force torque sensor. public: void SetForceTorqueSensor(const ForceTorque &_ft); @@ -312,6 +347,13 @@ namespace sdf /// \sa SensorType Type() const public: const ForceTorque *ForceTorqueSensor() const; + /// \brief Get a mutable force torque sensor, or nullptr if the sensor + /// does not contain a force torque sensor. + /// \return Pointer to the force torque sensor, or nullptr if the sensor + /// is not a force torque sensor. + /// \sa SensorType Type() const + public: ForceTorque *ForceTorqueSensor(); + /// \brief Set the IMU sensor. /// \param[in] _imu The IMU sensor. public: void SetImuSensor(const Imu &_imu); @@ -323,6 +365,13 @@ namespace sdf /// \sa SensorType Type() const public: const Imu *ImuSensor() const; + /// \brief Get a mutable IMU sensor, or nullptr if the sensor + /// does not contain an IMU sensor. + /// \return Pointer to the sensor's IMU, or nullptr if the sensor + /// is not an IMU. + /// \sa SensorType Type() const + public: Imu *ImuSensor(); + /// \brief Get the lidar sensor, or nullptr if this sensor type is not a /// Lidar. /// \return Pointer to the Lidar sensor, or nullptr if this Sensor is not a @@ -330,6 +379,13 @@ namespace sdf /// \sa SensorType Type() const public: const Lidar *LidarSensor() const; + /// \brief Get a mutable lidar sensor, or nullptr if this sensor type is + /// not a Lidar. + /// \return Pointer to the Lidar sensor, or nullptr if this Sensor is not a + /// Lidar. + /// \sa SensorType Type() const + public: Lidar *LidarSensor(); + /// \brief Set the lidar sensor. /// \param[in] _lidar The lidar sensor. public: void SetLidarSensor(const Lidar &_lidar); diff --git a/src/Sensor.cc b/src/Sensor.cc index 6f1ac7b3f..eed455cc6 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -512,6 +512,12 @@ const Magnetometer *Sensor::MagnetometerSensor() const return optionalToPointer(this->dataPtr->magnetometer); } +///////////////////////////////////////////////// +Magnetometer *Sensor::MagnetometerSensor() +{ + return optionalToPointer(this->dataPtr->magnetometer); +} + ///////////////////////////////////////////////// void Sensor::SetMagnetometerSensor(const Magnetometer &_mag) { @@ -524,6 +530,12 @@ const Altimeter *Sensor::AltimeterSensor() const return optionalToPointer(this->dataPtr->altimeter); } +///////////////////////////////////////////////// +Altimeter *Sensor::AltimeterSensor() +{ + return optionalToPointer(this->dataPtr->altimeter); +} + ///////////////////////////////////////////////// void Sensor::SetAltimeterSensor(const Altimeter &_alt) { @@ -536,6 +548,12 @@ const AirPressure *Sensor::AirPressureSensor() const return optionalToPointer(this->dataPtr->airPressure); } +///////////////////////////////////////////////// +AirPressure *Sensor::AirPressureSensor() +{ + return optionalToPointer(this->dataPtr->airPressure); +} + ///////////////////////////////////////////////// void Sensor::SetAirPressureSensor(const AirPressure &_air) { @@ -548,6 +566,12 @@ const Lidar *Sensor::LidarSensor() const return optionalToPointer(this->dataPtr->lidar); } +///////////////////////////////////////////////// +Lidar *Sensor::LidarSensor() +{ + return optionalToPointer(this->dataPtr->lidar); +} + ///////////////////////////////////////////////// void Sensor::SetLidarSensor(const Lidar &_lidar) { @@ -587,6 +611,12 @@ const Camera *Sensor::CameraSensor() const return optionalToPointer(this->dataPtr->camera); } +///////////////////////////////////////////////// +Camera *Sensor::CameraSensor() +{ + return optionalToPointer(this->dataPtr->camera); +} + ///////////////////////////////////////////////// void Sensor::SetForceTorqueSensor(const ForceTorque &_ft) { @@ -599,6 +629,12 @@ const ForceTorque *Sensor::ForceTorqueSensor() const return optionalToPointer(this->dataPtr->forceTorque); } +///////////////////////////////////////////////// +ForceTorque *Sensor::ForceTorqueSensor() +{ + return optionalToPointer(this->dataPtr->forceTorque); +} + ///////////////////////////////////////////////// void Sensor::SetNavSatSensor(const NavSat &_gps) { @@ -611,6 +647,12 @@ const NavSat *Sensor::NavSatSensor() const return optionalToPointer(this->dataPtr->navSat); } +///////////////////////////////////////////////// +NavSat *Sensor::NavSatSensor() +{ + return optionalToPointer(this->dataPtr->navSat); +} + ///////////////////////////////////////////////// void Sensor::SetImuSensor(const Imu &_imu) { @@ -622,3 +664,9 @@ const Imu *Sensor::ImuSensor() const { return optionalToPointer(this->dataPtr->imu); } + +///////////////////////////////////////////////// +Imu *Sensor::ImuSensor() +{ + return optionalToPointer(this->dataPtr->imu); +} diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index e1b66b92e..28ef3cb77 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -336,3 +336,167 @@ TEST(DOMSensor, EnableMetrics) sensor.SetEnableMetrics(false); EXPECT_EQ(false, sensor.EnableMetrics()); } + +///////////////////////////////////////////////// +TEST(DOMSensor, MutableSensors) +{ + // Altimeter + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::ALTIMETER); + + sdf::Altimeter alt; + sensor.SetAltimeterSensor(alt); + + sdf::Altimeter *altMutable = sensor.AltimeterSensor(); + ASSERT_NE(nullptr, altMutable); + EXPECT_DOUBLE_EQ(altMutable->VerticalPositionNoise().Mean(), + sensor.AltimeterSensor()->VerticalPositionNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + altMutable->SetVerticalPositionNoise(noise); + EXPECT_DOUBLE_EQ(altMutable->VerticalPositionNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.AltimeterSensor()->VerticalPositionNoise().Mean(), 2.0); + } + + // Air pressure + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::AIR_PRESSURE); + + sdf::AirPressure air; + sensor.SetAirPressureSensor(air); + + sdf::AirPressure *airMutable = sensor.AirPressureSensor(); + ASSERT_NE(nullptr, airMutable); + EXPECT_DOUBLE_EQ(airMutable->ReferenceAltitude(), + sensor.AirPressureSensor()->ReferenceAltitude()); + + airMutable->SetReferenceAltitude(2.0); + EXPECT_DOUBLE_EQ(airMutable->ReferenceAltitude(), 2.0); + EXPECT_DOUBLE_EQ(sensor.AirPressureSensor()->ReferenceAltitude(), 2.0); + } + + // Camera + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::CAMERA); + + sdf::Camera cam; + sensor.SetCameraSensor(cam); + + sdf::Camera *camMutable = sensor.CameraSensor(); + ASSERT_NE(nullptr, camMutable); + EXPECT_DOUBLE_EQ(camMutable->NearClip(), sensor.CameraSensor()->NearClip()); + + camMutable->SetNearClip(2.0); + EXPECT_DOUBLE_EQ(camMutable->NearClip(), 2.0); + EXPECT_DOUBLE_EQ(sensor.CameraSensor()->NearClip(), 2.0); + } + + // Force torque + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::FORCE_TORQUE); + + sdf::ForceTorque ftq; + sensor.SetForceTorqueSensor(ftq); + + sdf::ForceTorque *ftqMutable = sensor.ForceTorqueSensor(); + ASSERT_NE(nullptr, ftqMutable); + EXPECT_DOUBLE_EQ(ftqMutable->ForceXNoise().Mean(), + sensor.ForceTorqueSensor()->ForceXNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + ftqMutable->SetForceXNoise(noise); + EXPECT_DOUBLE_EQ(ftqMutable->ForceXNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.ForceTorqueSensor()->ForceXNoise().Mean(), 2.0); + } + + // IMU + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::FORCE_TORQUE); + + sdf::Imu imu; + sensor.SetImuSensor(imu); + + sdf::Imu *imuMutable = sensor.ImuSensor(); + ASSERT_NE(nullptr, imuMutable); + EXPECT_DOUBLE_EQ(imuMutable->LinearAccelerationXNoise().Mean(), + sensor.ImuSensor()->LinearAccelerationXNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + imuMutable->SetLinearAccelerationXNoise(noise); + EXPECT_DOUBLE_EQ(imuMutable->LinearAccelerationXNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.ImuSensor()->LinearAccelerationXNoise().Mean(), 2.0); + } + + // Lidar + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::LIDAR); + + sdf::Lidar ldr; + sensor.SetLidarSensor(ldr); + + sdf::Lidar *ldrMutable = sensor.LidarSensor(); + ASSERT_NE(nullptr, ldrMutable); + EXPECT_DOUBLE_EQ(ldrMutable->LidarNoise().Mean(), + sensor.LidarSensor()->LidarNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + ldrMutable->SetLidarNoise(noise); + EXPECT_DOUBLE_EQ(ldrMutable->LidarNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.LidarSensor()->LidarNoise().Mean(), 2.0); + } + + // Magnetometer + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::MAGNETOMETER); + + sdf::Magnetometer mag; + sensor.SetMagnetometerSensor(mag); + + sdf::Magnetometer *magMutable = sensor.MagnetometerSensor(); + ASSERT_NE(nullptr, magMutable); + EXPECT_DOUBLE_EQ(magMutable->XNoise().Mean(), + sensor.MagnetometerSensor()->XNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + magMutable->SetXNoise(noise); + EXPECT_DOUBLE_EQ(magMutable->XNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ(sensor.MagnetometerSensor()->XNoise().Mean(), 2.0); + } + + // NavSat + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::NAVSAT); + + sdf::NavSat nav; + sensor.SetNavSatSensor(nav); + + sdf::NavSat *navMutable = sensor.NavSatSensor(); + ASSERT_NE(nullptr, navMutable); + EXPECT_DOUBLE_EQ(navMutable->HorizontalPositionNoise().Mean(), + sensor.NavSatSensor()->HorizontalPositionNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + navMutable->SetHorizontalPositionNoise(noise); + EXPECT_DOUBLE_EQ(navMutable->HorizontalPositionNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.NavSatSensor()->HorizontalPositionNoise().Mean(), 2.0); + } +} From f805c954f2116b1d3a196f1d811b7a61eb9f6c4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 3 Nov 2021 18:12:18 +0100 Subject: [PATCH 26/60] Prepare for v9.7.0 (#739) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Prepare for v9.7.0 Signed-off-by: Carlos Agüero --- CMakeLists.txt | 4 ++-- Changelog.md | 17 +++++++++++++++++ 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 393c91368..138565b79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,8 +27,8 @@ project (sdformat9) set (SDF_PROTOCOL_VERSION 1.7) set (SDF_MAJOR_VERSION 9) -set (SDF_MINOR_VERSION 6) -set (SDF_PATCH_VERSION 1) +set (SDF_MINOR_VERSION 7) +set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}) diff --git a/Changelog.md b/Changelog.md index 4c784e072..9d64ecec7 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,23 @@ ### libsdformat 9.X.X (202X-XX-XX) +### libsdformat 9.7.0 (2021-11-03) + +1. Make exception for plugins when checking for name uniqueness + * [Pull request #733](https://github.com/ignitionrobotics/sdformat/pull/733) + +1. Backport test utilities from sdf10 + * [Pull request #731](https://github.com/ignitionrobotics/sdformat/pull/731) + +1. Added Force Torque Noise functions + Unit tests + * [Pull request #669](https://github.com/ignitionrobotics/sdformat/pull/669) + +1. Add Joint DOM API to access joint sensors + * [Pull request #517](https://github.com/ignitionrobotics/sdformat/pull/517) + +1. Add force torque sensor + * [Pull request #393](https://github.com/ignitionrobotics/sdformat/pull/393) + ### libsdformat 9.6.1 (2021-09-07) 1. Parse URDF continuous joint effort/velocity limits From 2905643a082527427e140595ad856e7c1032f642 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Thu, 4 Nov 2021 15:36:27 -0600 Subject: [PATCH 27/60] remove outdated deprecation note from parser_urdf.hh (#740) Signed-off-by: Ashton Larkin --- src/parser_urdf.hh | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/parser_urdf.hh b/src/parser_urdf.hh index 8f703169c..a5b960cbd 100644 --- a/src/parser_urdf.hh +++ b/src/parser_urdf.hh @@ -33,10 +33,6 @@ namespace sdf // /// \brief URDF to SDF converter - /// - /// This is now deprecated for external usage and will be removed in the next - /// major version of libsdformat. Instead, consider using `sdf::readFile` or - /// `sdf::readString`, which automatically convert URDF to SDF. class URDF2SDF { /// \brief constructor From 331ea5386a373fef562280e98023199c0d635aa3 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 5 Nov 2021 16:53:02 -0700 Subject: [PATCH 28/60] Fix test file path in fixed_joint_reduction.cc Signed-off-by: Steve Peters --- test/integration/fixed_joint_reduction.cc | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index 6dfbfd5e0..58e5df7ed 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -36,6 +36,8 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT[] = "fixed_joint_reduction_collision_visual_empty_root.urdf"; const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] = "fixed_joint_reduction_collision_visual_empty_root.sdf"; +const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] = + "fixed_joint_reduction_plugin_frame_extension.urdf"; static std::string GetFullTestFilePath(const char *_input) { @@ -744,7 +746,9 @@ TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest) { sdf::SDFPtr robot(new sdf::SDF()); sdf::init(robot); - ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION, robot)); + ASSERT_TRUE(sdf::readFile( + GetFullTestFilePath(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION), + robot)); sdf::ElementPtr model = robot->Root()->GetElement("model"); sdf::ElementPtr plugin = model->GetElement("plugin"); From d6fab5984a3b5ae0e8d6a551babad4b4a38c3c2c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 5 Nov 2021 16:53:41 -0700 Subject: [PATCH 29/60] Use SemanticPose API in test Signed-off-by: Steve Peters --- test/integration/frame.cc | 40 ++++++++++++++++++++++++++------------- 1 file changed, 27 insertions(+), 13 deletions(-) diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 97d9ce4d9..fd1b89080 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -1323,10 +1323,13 @@ TEST(Frame, IncludeFrameWithSubmodel) << "" << ""; - sdf::SDFPtr sdfParsed(new sdf::SDF()); - sdf::init(sdfParsed); - ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed)); - sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world"); + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors[0].Message(); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + ASSERT_EQ(1u, world->ModelCount()); /* top level model: using include will merge top_level_model and box_with_submodel into: * @@ -1338,11 +1341,18 @@ TEST(Frame, IncludeFrameWithSubmodel) * * */ - sdf::ElementPtr modelElem = worldElem->GetElement("model"); - EXPECT_EQ(modelElem->Get("name"), "top_level_model"); - sdf::ElementPtr modelPoseElem = - modelElem->GetElement("link")->GetElement("pose"); - EXPECT_EQ(modelPoseElem->Get(), + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ(model->Name(), "top_level_model"); + + const sdf::Link *link0 = model->LinkByIndex(0); + ASSERT_NE(nullptr, link0); + EXPECT_EQ(link0->Name(), "box_with_submodel::link"); + + ignition::math::Pose3d linkPose; + sdf::Errors resolveErrors = link0->SemanticPose().Resolve(linkPose); + EXPECT_TRUE(resolveErrors.empty()) << resolveErrors[0].Message(); + EXPECT_EQ(linkPose, ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); /* submodel: pose from parent is translated to model. links are the same * ... @@ -1353,10 +1363,14 @@ TEST(Frame, IncludeFrameWithSubmodel) * 5 5 0 0 -0 0 * */ - sdf::ElementPtr subModelElem = modelElem->GetElement("model"); - EXPECT_EQ(subModelElem->Get("name"), + const sdf::Model *submodel = model->ModelByIndex(0); + ASSERT_NE(nullptr, submodel); + EXPECT_EQ(submodel->Name(), "box_with_submodel::submodel_of_box_with_submodel"); - sdf::ElementPtr subModelPoseElem = subModelElem->GetElement("pose"); - EXPECT_EQ(subModelPoseElem->Get(), + + ignition::math::Pose3d submodelPose; + resolveErrors = submodel->SemanticPose().Resolve(submodelPose); + EXPECT_TRUE(resolveErrors.empty()) << resolveErrors[0].Message(); + EXPECT_EQ(submodelPose, ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); } From 38be09c68fd15e308710717e02f128cca9a1d4da Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 5 Nov 2021 16:59:58 -0700 Subject: [PATCH 30/60] Include nested model test Signed-off-by: Steve Peters --- test/integration/nested_model.cc | 48 ++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 29ba5b8b9..e25053537 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -243,6 +243,54 @@ TEST(NestedModel, State) ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); } +//////////////////////////////////////// +// Test parsing a include element that has a pose element and includes a +// submodel +TEST(NestedModel, IncludeFlatteningNames) +{ + const std::string MODEL_PATH = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", + "model", "nested_names_test"); + + std::ostringstream stream; + std::string version = SDF_VERSION; + stream + << "" + << "" + << " " + << " " + MODEL_PATH +"" + << " " + << "" + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed)); + + sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model"); + EXPECT_EQ(modelElem->Get("name"), "top_level_model"); + + sdf::ElementPtr linkElem = modelElem->GetElement("link"); + EXPECT_EQ(linkElem->Get("name"), "main_model_prefix::frame"); + + sdf::ElementPtr jointElem = modelElem->GetElement("joint"); + EXPECT_EQ(jointElem->Get("name"), "main_model_prefix::joint1"); + EXPECT_EQ(jointElem->Get("parent"), + "main_model_prefix::subnested_model"); + EXPECT_EQ(jointElem->Get("child"), + "main_model_prefix::subnested_model::link1") << + "Flattening logic for nested models failed (check parser.cc)"; + + sdf::ElementPtr joint2Elem = jointElem->GetNextElement("joint"); + EXPECT_EQ(joint2Elem->Get("name"), "main_model_prefix::joint2"); + EXPECT_EQ(joint2Elem->Get("parent"), + "main_model_prefix::subnested_model::link1") << + "Flattening logic for nested models failed (check parser.cc)"; + EXPECT_EQ(joint2Elem->Get("child"), + "main_model_prefix::joint1") << + "Flattening logic for nested models failed (check parser.cc)"; +} + //////////////////////////////////////// // Test parsing models with joints nested via // Confirm that joint axis rotation is handled differently for 1.4 and 1.5+ From 283d4d3274576f5b7d64048b098e5ceb0ab62111 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20Wallk=C3=B6tter?= Date: Tue, 9 Nov 2021 16:28:51 +0100 Subject: [PATCH 31/60] Documentation: Only allow one canonical_link attribute for model (#716) Signed-off-by: FirefoxMetzger Co-authored-by: Steve Peters Co-authored-by: Addisu Z. Taddese --- sdf/1.7/model.sdf | 2 +- sdf/1.8/model.sdf | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/sdf/1.7/model.sdf b/sdf/1.7/model.sdf index ce959bbeb..80d9a5940 100644 --- a/sdf/1.7/model.sdf +++ b/sdf/1.7/model.sdf @@ -11,7 +11,7 @@ - + The name of the model's canonical link, to which the model's implicit coordinate frame is attached. If unset or set to an empty string, diff --git a/sdf/1.8/model.sdf b/sdf/1.8/model.sdf index 4cb1cf3c6..ce38709a3 100644 --- a/sdf/1.8/model.sdf +++ b/sdf/1.8/model.sdf @@ -11,7 +11,7 @@ - + The name of the model's canonical link, to which the model's implicit coordinate frame is attached. If unset or set to an empty string, From 6307392809541d15bd540e3db9dcd98b51431a3a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 9 Nov 2021 14:14:29 -0800 Subject: [PATCH 32/60] Prepare for 12.1.0 release (#743) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- Changelog.md | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a7370d855..22dd03fe0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ project (sdformat12) set (SDF_PROTOCOL_VERSION 1.9) set (SDF_MAJOR_VERSION 12) -set (SDF_MINOR_VERSION 0) +set (SDF_MINOR_VERSION 1) set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) diff --git a/Changelog.md b/Changelog.md index 49da122bf..46cc3d6b9 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,10 @@ ## libsdformat 12.X +### libsdformat 12.1.0 (2021-11-09) + +1. Support accessing mutable sensor types. + * [Pull request #737](https://github.com/ignitionrobotics/sdformat/pull/737) + ### libsdformat 12.0.0 (2021-09-30) 1. Make exception for plugins when checking for name uniqueness From 424bf9c8d1213eda60c108c75e2c48c0b0eeab7f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 11 Nov 2021 17:37:31 -0800 Subject: [PATCH 33/60] Fix URDF fixed joint reduction of plugins (#745) There is special handling in the parser_urdf code to update plugin fields when links are merged together during fixed joint reduction. A test for this was added to sdf6 in #500. A portion of this test is applied directly to the sdf10 branch to illustrate a problem with ReduceSDFExtensionPluginFrameReplace in parser_urdf.cc. The original migration to use tinyxml2 in #264 changed the data structure used in SDFExtension to store XMLDocuments instead of XMLPointers, which requires an extra call to FirstChildElement, but the ReduceSDFExtension*FrameReplace functions did not receive this update. The fix here refactors the function arguments to pass the first child element directly, which simplifies the helper function implementation. Signed-off-by: Steve Peters --- src/parser_urdf.cc | 36 +++--- test/integration/fixed_joint_reduction.cc | 20 ++++ ...oint_reduction_plugin_frame_extension.urdf | 103 ++++++++++++++++++ 3 files changed, 141 insertions(+), 18 deletions(-) create mode 100644 test/integration/fixed_joint_reduction_plugin_frame_extension.urdf diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 3187c568f..b1d3ecf91 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -183,7 +183,7 @@ void ReduceSDFExtensionProjectorFrameReplace( /// reduced fixed joints: apply appropriate frame updates in plugins /// inside urdf extensions when doing fixed joint reduction void ReduceSDFExtensionPluginFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link, const std::string &_pluginName, const std::string &_elementName, ignition::math::Pose3d _reductionTransform); @@ -2579,12 +2579,12 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge, << debugStreamIn.CStr() << "]\n"; ReduceSDFExtensionContactSensorFrameReplace(blobIt, _link); - ReduceSDFExtensionPluginFrameReplace(blobIt, _link, - "plugin", "bodyName", - _ge->reductionTransform); - ReduceSDFExtensionPluginFrameReplace(blobIt, _link, - "plugin", "frameName", - _ge->reductionTransform); + ReduceSDFExtensionPluginFrameReplace( + (*blobIt)->FirstChildElement(), _link, "plugin", "bodyName", + _ge->reductionTransform); + ReduceSDFExtensionPluginFrameReplace( + (*blobIt)->FirstChildElement(), _link, "plugin", "frameName", + _ge->reductionTransform); ReduceSDFExtensionProjectorFrameReplace(blobIt, _link); ReduceSDFExtensionGripperFrameReplace(blobIt, _link); ReduceSDFExtensionJointFrameReplace(blobIt, _link); @@ -3581,24 +3581,24 @@ void ReduceSDFExtensionContactSensorFrameReplace( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionPluginFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link, const std::string &_pluginName, const std::string &_elementName, ignition::math::Pose3d _reductionTransform) { std::string linkName = _link->name; std::string parentLinkName = _link->getParent()->name; - if ((*_blobIt)->FirstChildElement()->Name() == _pluginName) + if (_blob->Name() == _pluginName) { // replace element containing _link names to parent link names // find first instance of xyz and rpy, replace with reduction transform tinyxml2::XMLNode *elementNode = - (*_blobIt)->FirstChildElement(_elementName.c_str()); + _blob->FirstChildElement(_elementName.c_str()); if (elementNode) { if (GetKeyValueAsString(elementNode->ToElement()) == linkName) { - (*_blobIt)->DeleteChild(elementNode); + _blob->DeleteChild(elementNode); auto* doc = elementNode->GetDocument(); tinyxml2::XMLElement *bodyNameKey = doc->NewElement(_elementName.c_str()); @@ -3607,27 +3607,27 @@ void ReduceSDFExtensionPluginFrameReplace( tinyxml2::XMLText *bodyNameTxt = doc->NewText(bodyNameStream.str().c_str()); bodyNameKey->LinkEndChild(bodyNameTxt); - (*_blobIt)->LinkEndChild(bodyNameKey); + _blob->LinkEndChild(bodyNameKey); /// @todo update transforms for this sdf plugin too // look for offset transforms, add reduction transform - tinyxml2::XMLNode *xyzKey = (*_blobIt)->FirstChildElement("xyzOffset"); + tinyxml2::XMLNode *xyzKey = _blob->FirstChildElement("xyzOffset"); if (xyzKey) { urdf::Vector3 v1 = ParseVector3(xyzKey); _reductionTransform.Pos() = ignition::math::Vector3d(v1.x, v1.y, v1.z); // remove xyzOffset and rpyOffset - (*_blobIt)->DeleteChild(xyzKey); + _blob->DeleteChild(xyzKey); } - tinyxml2::XMLNode *rpyKey = (*_blobIt)->FirstChildElement("rpyOffset"); + tinyxml2::XMLNode *rpyKey = _blob->FirstChildElement("rpyOffset"); if (rpyKey) { urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0); _reductionTransform.Rot() = ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z); // remove xyzOffset and rpyOffset - (*_blobIt)->DeleteChild(rpyKey); + _blob->DeleteChild(rpyKey); } // pass through the parent transform from fixed joint reduction @@ -3661,8 +3661,8 @@ void ReduceSDFExtensionPluginFrameReplace( xyzKey->LinkEndChild(xyzTxt); rpyKey->LinkEndChild(rpyTxt); - (*_blobIt)->LinkEndChild(xyzKey); - (*_blobIt)->LinkEndChild(rpyKey); + _blob->LinkEndChild(xyzKey); + _blob->LinkEndChild(rpyKey); } } } diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index 0475db78b..d1780b0d9 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -36,6 +36,8 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT[] = "fixed_joint_reduction_collision_visual_empty_root.urdf"; const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] = "fixed_joint_reduction_collision_visual_empty_root.sdf"; +const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] = + "fixed_joint_reduction_plugin_frame_extension.urdf"; static std::string GetFullTestFilePath(const char *_input) { @@ -734,3 +736,21 @@ TEST(SDFParser, FixedJointReductionSimple) EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance); } } + +///////////////////////////////////////////////// +// This test uses a urdf that has chained fixed joints with plugin that +// contains bodyName, xyzOffset and rpyOffset. +// Test to make sure that the bodyName is updated to the new base link. +TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest) +{ + sdf::SDFPtr robot(new sdf::SDF()); + sdf::init(robot); + ASSERT_TRUE(sdf::readFile( + GetFullTestFilePath(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION), robot)); + + sdf::ElementPtr model = robot->Root()->GetElement("model"); + sdf::ElementPtr plugin = model->GetElement("plugin"); + + auto bodyName = plugin->Get("bodyName"); + EXPECT_EQ("base_link", bodyName); +} diff --git a/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf b/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf new file mode 100644 index 000000000..4c2293670 --- /dev/null +++ b/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf @@ -0,0 +1,103 @@ + + + + + /test/plugin/service + /test/plugin/topic + link2 + 100 + 0 0 0 + 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From ceea6a5b5502e2985cb3d36c561085c4b0a844e6 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 12 Nov 2021 16:33:03 -0800 Subject: [PATCH 34/60] Adapt IncludeFrameWithSubmodel test to sdf11 Signed-off-by: Steve Peters --- test/integration/frame.cc | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/test/integration/frame.cc b/test/integration/frame.cc index b721167cf..90b15be93 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -1300,14 +1300,15 @@ TEST(Frame, IncludeFrameWithSubmodel) ASSERT_NE(nullptr, model); EXPECT_EQ(model->Name(), "top_level_model"); - const sdf::Link *link0 = model->LinkByIndex(0); + const sdf::Link *link0 = model->LinkByName("box_with_submodel::link"); ASSERT_NE(nullptr, link0); - EXPECT_EQ(link0->Name(), "box_with_submodel::link"); + EXPECT_EQ(link0->Name(), "link"); ignition::math::Pose3d linkPose; - sdf::Errors resolveErrors = link0->SemanticPose().Resolve(linkPose); + sdf::Errors resolveErrors = model->SemanticPose().Resolve(linkPose, + "top_level_model::box_with_submodel::link"); EXPECT_TRUE(resolveErrors.empty()) << resolveErrors[0].Message(); - EXPECT_EQ(linkPose, + EXPECT_EQ(linkPose.Inverse(), ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); /* submodel: pose from parent is translated to model. links are the same * ... @@ -1318,14 +1319,16 @@ TEST(Frame, IncludeFrameWithSubmodel) * 5 5 0 0 -0 0 * */ - const sdf::Model *submodel = model->ModelByIndex(0); + const sdf::Model *submodel = model->ModelByName( + "box_with_submodel::submodel_of_box_with_submodel"); ASSERT_NE(nullptr, submodel); EXPECT_EQ(submodel->Name(), - "box_with_submodel::submodel_of_box_with_submodel"); + "submodel_of_box_with_submodel"); ignition::math::Pose3d submodelPose; - resolveErrors = submodel->SemanticPose().Resolve(submodelPose); + resolveErrors = model->SemanticPose().Resolve(submodelPose, + "top_level_model::box_with_submodel"); EXPECT_TRUE(resolveErrors.empty()) << resolveErrors[0].Message(); - EXPECT_EQ(submodelPose, + EXPECT_EQ(submodelPose.Inverse(), ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); } From 9cecc756ac13e594d765328677c3fac9684a12af Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 12 Nov 2021 16:34:27 -0800 Subject: [PATCH 35/60] Remove outdated IncludeFlatteningNames test/model The IncludeFlatteningNames test added in #597 is no longer relevant because nested model names are no longer flattened. The model used by the test is removed as well since it is not used elsewhere. Signed-off-by: Steve Peters --- .../model/nested_names_test/model.config | 14 ------ .../model/nested_names_test/submodel_test.sdf | 21 -------- test/integration/nested_model.cc | 48 ------------------- 3 files changed, 83 deletions(-) delete mode 100644 test/integration/model/nested_names_test/model.config delete mode 100644 test/integration/model/nested_names_test/submodel_test.sdf diff --git a/test/integration/model/nested_names_test/model.config b/test/integration/model/nested_names_test/model.config deleted file mode 100644 index 6bb3d1f60..000000000 --- a/test/integration/model/nested_names_test/model.config +++ /dev/null @@ -1,14 +0,0 @@ - - - - Submodel - 0.1.0 - submodel_test.sdf - - Christina Gomez - cgomez@swri.org - - - A hinged door with two handles - - diff --git a/test/integration/model/nested_names_test/submodel_test.sdf b/test/integration/model/nested_names_test/submodel_test.sdf deleted file mode 100644 index 2bd5d5294..000000000 --- a/test/integration/model/nested_names_test/submodel_test.sdf +++ /dev/null @@ -1,21 +0,0 @@ - - - - - 0.06 -0.0005 0 0 -0 0 - - - - - - - subnested_model - subnested_model::link1 - - - - main_model_prefix::subnested_model::link1 - joint1 - - - diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index b8916935b..d6056274d 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -243,54 +243,6 @@ TEST(NestedModel, State) ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); } -//////////////////////////////////////// -// Test parsing a include element that has a pose element and includes a -// submodel -TEST(NestedModel, IncludeFlatteningNames) -{ - const std::string MODEL_PATH = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "model", "nested_names_test"); - - std::ostringstream stream; - std::string version = SDF_VERSION; - stream - << "" - << "" - << " " - << " " + MODEL_PATH +"" - << " " - << "" - << ""; - - sdf::SDFPtr sdfParsed(new sdf::SDF()); - sdf::init(sdfParsed); - ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed)); - - sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model"); - EXPECT_EQ(modelElem->Get("name"), "top_level_model"); - - sdf::ElementPtr linkElem = modelElem->GetElement("link"); - EXPECT_EQ(linkElem->Get("name"), "main_model_prefix::frame"); - - sdf::ElementPtr jointElem = modelElem->GetElement("joint"); - EXPECT_EQ(jointElem->Get("name"), "main_model_prefix::joint1"); - EXPECT_EQ(jointElem->Get("parent"), - "main_model_prefix::subnested_model"); - EXPECT_EQ(jointElem->Get("child"), - "main_model_prefix::subnested_model::link1") << - "Flattening logic for nested models failed (check parser.cc)"; - - sdf::ElementPtr joint2Elem = jointElem->GetNextElement("joint"); - EXPECT_EQ(joint2Elem->Get("name"), "main_model_prefix::joint2"); - EXPECT_EQ(joint2Elem->Get("parent"), - "main_model_prefix::subnested_model::link1") << - "Flattening logic for nested models failed (check parser.cc)"; - EXPECT_EQ(joint2Elem->Get("child"), - "main_model_prefix::joint1") << - "Flattening logic for nested models failed (check parser.cc)"; -} - //////////////////////////////////////// // Test parsing models with joints nested via // Confirm that joint axis rotation is handled differently for 1.4 and 1.5+ From db26a607ebddb42fddb50bf41ad627d7291c536c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 12 Nov 2021 16:41:10 -0800 Subject: [PATCH 36/60] Copy sdf/1.7/forcetorque.sdf noise elements to 1.8 Signed-off-by: Steve Peters --- sdf/1.7/forcetorque.sdf | 4 ++-- sdf/1.8/forcetorque.sdf | 34 ++++++++++++++++++++++++++++++++++ 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/sdf/1.7/forcetorque.sdf b/sdf/1.7/forcetorque.sdf index 115d34c6f..5a1846951 100644 --- a/sdf/1.7/forcetorque.sdf +++ b/sdf/1.7/forcetorque.sdf @@ -19,7 +19,7 @@ - These elements are specific to measurement-frame force, + These elements are specific to measurement-frame force, which is expressed in Newtons Force along the X axis @@ -36,7 +36,7 @@ - These elements are specific to measurement-frame torque, + These elements are specific to measurement-frame torque, which is expressed in Newton-meters Torque about the X axis diff --git a/sdf/1.8/forcetorque.sdf b/sdf/1.8/forcetorque.sdf index 18e5b8f71..5a1846951 100644 --- a/sdf/1.8/forcetorque.sdf +++ b/sdf/1.8/forcetorque.sdf @@ -17,4 +17,38 @@ "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. + + + These elements are specific to measurement-frame force, + which is expressed in Newtons + + Force along the X axis + + + + Force along the Y axis + + + + Force along the Z axis + + + + + + These elements are specific to measurement-frame torque, + which is expressed in Newton-meters + + Torque about the X axis + + + + Force about the Y axis + + + + Torque about the Z axis + + + From ad00668fcd861ff966867416c62e982e1661741f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 16 Nov 2021 08:01:58 -0800 Subject: [PATCH 37/60] Add function to convert Sensor DOM to sdf::ElementPtr (#753) * add class to convert sensor dom to elem Signed-off-by: Ian Chen * tweak noise Signed-off-by: Ian Chen * Suggestion to move dom conversion to sensor classes (#754) * Suggestion to move dom conversion to sensor classes Signed-off-by: Nate Koenig * Remove file from smake Signed-off-by: Nate Koenig * remove header file in cmake Signed-off-by: Ian Chen * Fixing build Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Ian Chen Co-authored-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/AirPressure.hh | 5 + include/sdf/Altimeter.hh | 5 + include/sdf/Camera.hh | 9 + include/sdf/ForceTorque.hh | 5 + include/sdf/Imu.hh | 5 + include/sdf/Lidar.hh | 5 + include/sdf/Magnetometer.hh | 5 + include/sdf/Noise.hh | 8 + include/sdf/Sensor.hh | 5 + src/AirPressure.cc | 10 + src/Altimeter.cc | 15 + src/Camera.cc | 91 ++++++ src/Camera_TEST.cc | 2 + src/ForceTorque.cc | 76 +++++ src/Imu.cc | 60 ++++ src/Lidar.cc | 32 ++ src/Magnetometer.cc | 25 ++ src/Noise.cc | 42 +++ src/Sensor.cc | 65 ++++ test/integration/CMakeLists.txt | 1 + test/integration/sdf_dom_conversion.cc | 404 +++++++++++++++++++++++++ test/sdf/sensors.sdf | 21 ++ 22 files changed, 896 insertions(+) create mode 100644 test/integration/sdf_dom_conversion.cc diff --git a/include/sdf/AirPressure.hh b/include/sdf/AirPressure.hh index 05f656a5d..aee47149d 100644 --- a/include/sdf/AirPressure.hh +++ b/include/sdf/AirPressure.hh @@ -84,6 +84,11 @@ namespace sdf /// \returen True if 'this' != _mag. public: bool operator!=(const AirPressure &_air) const; + /// \brief Fill the provided _elem with data from this sensor type. + /// \param[out] _elem SDF element point to populate + /// \return True if successful. + public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Altimeter.hh b/include/sdf/Altimeter.hh index 03bbbf0fa..5452da1b0 100644 --- a/include/sdf/Altimeter.hh +++ b/include/sdf/Altimeter.hh @@ -76,6 +76,11 @@ namespace sdf /// \returen True if 'this' != _alt. public: bool operator!=(const Altimeter &_alt) const; + /// \brief Fill the provided _elem with data from this sensor type. + /// \param[out] _elem SDF element point to populate + /// \return True if successful. + public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 054822323..ae07199dc 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -468,6 +468,15 @@ namespace sdf /// \param[in] _mask visibility mask public: void SetVisibilityMask(uint32_t _mask); + /// \brief Get whether or not the camera has instrinsics values set + /// \return True if the camera has instrinsics values set, false otherwise + public: bool HasLensIntrinsics() const; + + /// \brief Fill the provided _elem with data from this sensor type. + /// \param[out] _elem SDF element point to populate + /// \return True if successful. + public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh index 172e63ade..b5b9000d4 100644 --- a/include/sdf/ForceTorque.hh +++ b/include/sdf/ForceTorque.hh @@ -154,6 +154,11 @@ namespace sdf /// \returen True if 'this' != _ft. public: bool operator!=(const ForceTorque &_ft) const; + /// \brief Fill the provided _elem with data from this sensor type. + /// \param[out] _elem SDF element point to populate + /// \return True if successful. + public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index 85b690701..93ac4e91c 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -253,6 +253,11 @@ namespace sdf /// \returen True if 'this' != _imu. public: bool operator!=(const Imu &_imu) const; + /// \brief Fill the provided _elem with data from this sensor type. + /// \param[out] _elem SDF element point to populate + /// \return True if successful. + public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Lidar.hh b/include/sdf/Lidar.hh index 1595fb8bd..16d0cbfd5 100644 --- a/include/sdf/Lidar.hh +++ b/include/sdf/Lidar.hh @@ -232,6 +232,11 @@ namespace sdf /// \return True if 'this' != _lidar. public: bool operator!=(const Lidar &_lidar) const; + /// \brief Fill the provided _elem with data from this sensor type. + /// \param[out] _elem SDF element point to populate + /// \return True if successful. + public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Magnetometer.hh b/include/sdf/Magnetometer.hh index 642960dfe..ed7406c33 100644 --- a/include/sdf/Magnetometer.hh +++ b/include/sdf/Magnetometer.hh @@ -85,6 +85,11 @@ namespace sdf /// \returen True if 'this' != _mag. public: bool operator!=(const Magnetometer &_mag) const; + /// \brief Fill the provided _elem with data from this sensor type. + /// \param[out] _elem SDF element point to populate + /// \return True if successful. + public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Noise.hh b/include/sdf/Noise.hh index c0d8382f8..a124a23cb 100644 --- a/include/sdf/Noise.hh +++ b/include/sdf/Noise.hh @@ -162,6 +162,14 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Fill the provided _elem with data from this sensor type. + /// \param[out] _elem SDF element point to populate + /// \param[in] _simpleNoise True if the element should be populated with + /// just type, mean, and standard deviation. + /// \return True if successful. + public: bool PopulateElement(sdf::ElementPtr _elem, + bool _simpleNoise = false) const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index ef3ddbd08..bbde81403 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -203,6 +203,11 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Fill the provided _elem with data from this sensor. + /// \param[out] _elem SDF element point to populate + /// \return True if successful. + public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Get the sensor type. /// \return The sensor type. public: SensorType Type() const; diff --git a/src/AirPressure.cc b/src/AirPressure.cc index 44d76a48c..9ed186200 100644 --- a/src/AirPressure.cc +++ b/src/AirPressure.cc @@ -113,3 +113,13 @@ void AirPressure::SetPressureNoise(const Noise &_noise) { this->dataPtr->noise = _noise; } + +///////////////////////////////////////////////// +bool AirPressure::PopulateElement(sdf::ElementPtr _elem) const +{ + _elem->GetElement("reference_altitude")->Set( + this->ReferenceAltitude()); + sdf::ElementPtr pressureElem = _elem->GetElement("pressure"); + sdf::ElementPtr noiseElem = pressureElem->GetElement("noise"); + return this->dataPtr->noise.PopulateElement(noiseElem); +} diff --git a/src/Altimeter.cc b/src/Altimeter.cc index 6ec32bc24..a211a9b81 100644 --- a/src/Altimeter.cc +++ b/src/Altimeter.cc @@ -127,3 +127,18 @@ bool Altimeter::operator==(const Altimeter &_alt) const return true; } + +///////////////////////////////////////////////// +bool Altimeter::PopulateElement(sdf::ElementPtr _elem) const +{ + sdf::ElementPtr verticalPosElem = _elem->GetElement("vertical_position"); + sdf::ElementPtr verticalPosNoiseElem = verticalPosElem->GetElement("noise"); + + sdf::ElementPtr verticalVelElem = _elem->GetElement("vertical_velocity"); + sdf::ElementPtr verticalVelNoiseElem = verticalVelElem->GetElement("noise"); + + return + this->dataPtr->verticalPositionNoise.PopulateElement(verticalPosNoiseElem) + && + this->dataPtr->verticalVelocityNoise.PopulateElement(verticalVelNoiseElem); +} diff --git a/src/Camera.cc b/src/Camera.cc index 027575e35..0f2bf13d5 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -174,6 +174,9 @@ class sdf::Camera::Implementation /// \brief lens instrinsics s. public: double lensIntrinsicsS{1.0}; + /// \brief True if this camera has custom intrinsics values + public: bool hasIntrinsics = false; + /// \brief Visibility mask of a camera. Defaults to 0xFFFFFFFF public: uint32_t visibilityMask{4294967295u}; }; @@ -367,6 +370,7 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->lensIntrinsicsCy).first; this->dataPtr->lensIntrinsicsS = intrinsics->Get("s", this->dataPtr->lensIntrinsicsS).first; + this->dataPtr->hasIntrinsics = true; } } @@ -865,6 +869,7 @@ double Camera::LensIntrinsicsFx() const void Camera::SetLensIntrinsicsFx(double _fx) { this->dataPtr->lensIntrinsicsFx = _fx; + this->dataPtr->hasIntrinsics = true; } ///////////////////////////////////////////////// @@ -877,6 +882,7 @@ double Camera::LensIntrinsicsFy() const void Camera::SetLensIntrinsicsFy(double _fy) { this->dataPtr->lensIntrinsicsFy = _fy; + this->dataPtr->hasIntrinsics = true; } ///////////////////////////////////////////////// @@ -889,6 +895,7 @@ double Camera::LensIntrinsicsCx() const void Camera::SetLensIntrinsicsCx(double _cx) { this->dataPtr->lensIntrinsicsCx = _cx; + this->dataPtr->hasIntrinsics = true; } ///////////////////////////////////////////////// @@ -901,6 +908,7 @@ double Camera::LensIntrinsicsCy() const void Camera::SetLensIntrinsicsCy(double _cy) { this->dataPtr->lensIntrinsicsCy = _cy; + this->dataPtr->hasIntrinsics = true; } ///////////////////////////////////////////////// @@ -913,6 +921,7 @@ double Camera::LensIntrinsicsSkew() const void Camera::SetLensIntrinsicsSkew(double _s) { this->dataPtr->lensIntrinsicsS = _s; + this->dataPtr->hasIntrinsics = true; } ///////////////////////////////////////////////// @@ -968,3 +977,85 @@ void Camera::SetVisibilityMask(uint32_t _mask) { this->dataPtr->visibilityMask = _mask; } + +///////////////////////////////////////////////// +bool Camera::HasLensIntrinsics() const +{ + return this->dataPtr->hasIntrinsics; +} + +///////////////////////////////////////////////// +bool Camera::PopulateElement(sdf::ElementPtr _elem) const +{ + _elem->GetAttribute("name")->Set(this->Name()); + _elem->GetElement("pose")->Set(this->RawPose()); + _elem->GetElement("horizontal_fov")->Set( + this->HorizontalFov().Radian()); + sdf::ElementPtr imageElem = _elem->GetElement("image"); + imageElem->GetElement("width")->Set(this->ImageWidth()); + imageElem->GetElement("height")->Set(this->ImageHeight()); + imageElem->GetElement("format")->Set(this->PixelFormatStr()); + sdf::ElementPtr clipElem = _elem->GetElement("clip"); + clipElem->GetElement("near")->Set(this->NearClip()); + clipElem->GetElement("far")->Set(this->FarClip()); + + if (this->dataPtr->hasDepthCamera) + { + sdf::ElementPtr depthElem = _elem->GetElement("depth_camera"); + sdf::ElementPtr depthClipElem = depthElem->GetElement("clip"); + depthClipElem->GetElement("near")->Set(this->DepthNearClip()); + depthClipElem->GetElement("far")->Set(this->DepthFarClip()); + } + + sdf::ElementPtr saveElem = _elem->GetElement("save"); + saveElem->GetAttribute("enabled")->Set(this->SaveFrames()); + if (this->SaveFrames()) + saveElem->GetElement("path")->Set(this->SaveFramesPath()); + + sdf::ElementPtr noiseElem = _elem->GetElement("noise"); + this->dataPtr->imageNoise.PopulateElement(noiseElem, true); + + sdf::ElementPtr distortionElem = _elem->GetElement("distortion"); + distortionElem->GetElement("k1")->Set(this->DistortionK1()); + distortionElem->GetElement("k2")->Set(this->DistortionK2()); + distortionElem->GetElement("k3")->Set(this->DistortionK3()); + distortionElem->GetElement("p1")->Set(this->DistortionP1()); + distortionElem->GetElement("p2")->Set(this->DistortionP2()); + distortionElem->GetElement("center")->Set( + this->DistortionCenter()); + + sdf::ElementPtr lensElem = _elem->GetElement("lens"); + lensElem->GetElement("type")->Set(this->LensType()); + lensElem->GetElement("scale_to_hfov")->Set(this->LensScaleToHfov()); + lensElem->GetElement("cutoff_angle")->Set( + this->LensCutoffAngle().Radian()); + lensElem->GetElement("env_texture_size")->Set( + this->LensEnvironmentTextureSize()); + if (this->LensType() == "custom") + { + sdf::ElementPtr customLensElem = lensElem->GetElement("custom_function"); + customLensElem->GetElement("c1")->Set(this->LensC1()); + customLensElem->GetElement("c2")->Set(this->LensC2()); + customLensElem->GetElement("c3")->Set(this->LensC3()); + customLensElem->GetElement("f")->Set(this->LensFocalLength()); + customLensElem->GetElement("fun")->Set( + this->LensFunction()); + } + if (this->HasLensIntrinsics()) + { + sdf::ElementPtr intrinsicsElem = lensElem->GetElement("intrinsics"); + intrinsicsElem->GetElement("fx")->Set(this->LensIntrinsicsFx()); + intrinsicsElem->GetElement("fy")->Set(this->LensIntrinsicsFy()); + intrinsicsElem->GetElement("cx")->Set(this->LensIntrinsicsCx()); + intrinsicsElem->GetElement("cy")->Set(this->LensIntrinsicsCy()); + intrinsicsElem->GetElement("s")->Set(this->LensIntrinsicsSkew()); + } + + if (this->HasSegmentationType()) + { + _elem->GetElement("segmentation_type")->Set( + this->SegmentationType()); + } + + return true; +} diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index f624315b2..9487a9692 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -174,6 +174,8 @@ TEST(DOMCamera, Construction) cam.SetLensIntrinsicsSkew(2.3); EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew()); + EXPECT_TRUE(cam.HasLensIntrinsics()); + EXPECT_EQ(4294967295u, cam.VisibilityMask()); cam.SetVisibilityMask(123u); EXPECT_EQ(123u, cam.VisibilityMask()); diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc index 01e3f0d63..dea4926e4 100644 --- a/src/ForceTorque.cc +++ b/src/ForceTorque.cc @@ -277,3 +277,79 @@ void ForceTorque::SetMeasureDirection( { this->dataPtr->measure_direction = _direction; } + +///////////////////////////////////////////////// +bool ForceTorque::PopulateElement(sdf::ElementPtr _elem) const +{ + bool result = true; + std::string frame; + switch (this->Frame()) + { + case sdf::ForceTorqueFrame::PARENT: + frame = "parent"; + break; + case sdf::ForceTorqueFrame::CHILD: + frame = "child"; + break; + case sdf::ForceTorqueFrame::SENSOR: + frame = "sensor"; + break; + case sdf::ForceTorqueFrame::INVALID: + default: + break; + } + if (!frame.empty()) + _elem->GetElement("frame")->Set(frame); + + std::string measureDirection; + switch (this->MeasureDirection()) + { + case sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD: + measureDirection = "parent_to_child"; + break; + case sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT: + measureDirection = "child_to_parent"; + break; + case sdf::ForceTorqueMeasureDirection::INVALID: + default: + break; + } + if (!measureDirection.empty()) + { + _elem->GetElement("measure_direction")->Set(measureDirection); + } + + sdf::ElementPtr forceElem = _elem->GetElement("force"); + sdf::ElementPtr forceXElem = forceElem->GetElement("x"); + sdf::ElementPtr forceXNoiseElem = forceXElem->GetElement("noise"); + result = result && + this->dataPtr->forceXNoise.PopulateElement(forceXNoiseElem); + + sdf::ElementPtr forceYElem = forceElem->GetElement("y"); + sdf::ElementPtr forceYNoiseElem = forceYElem->GetElement("noise"); + result = result && + this->dataPtr->forceYNoise.PopulateElement(forceYNoiseElem); + + sdf::ElementPtr forceZElem = forceElem->GetElement("z"); + sdf::ElementPtr forceZNoiseElem = forceZElem->GetElement("noise"); + result = result && + this->dataPtr->forceZNoise.PopulateElement(forceZNoiseElem); + + sdf::ElementPtr torqueElem = _elem->GetElement("torque"); + sdf::ElementPtr torqueXElem = torqueElem->GetElement("x"); + sdf::ElementPtr torqueXNoiseElem = torqueXElem->GetElement("noise"); + result = result && + this->dataPtr->torqueXNoise.PopulateElement(torqueXNoiseElem); + + sdf::ElementPtr torqueYElem = torqueElem->GetElement("y"); + sdf::ElementPtr torqueYNoiseElem = torqueYElem->GetElement("noise"); + result = result && + this->dataPtr->torqueYNoise.PopulateElement(torqueYNoiseElem); + + sdf::ElementPtr torqueZElem = torqueElem->GetElement("z"); + sdf::ElementPtr torqueZNoiseElem = torqueZElem->GetElement("noise"); + result = result && + this->dataPtr->torqueZNoise.PopulateElement(torqueZNoiseElem); + + return result; +} diff --git a/src/Imu.cc b/src/Imu.cc index 9512ada62..a51a39cc0 100644 --- a/src/Imu.cc +++ b/src/Imu.cc @@ -365,3 +365,63 @@ bool Imu::OrientationEnabled() const { return this->dataPtr->orientationEnabled; } + +///////////////////////////////////////////////// +bool Imu::PopulateElement(sdf::ElementPtr _elem) const +{ + bool result = true; + + sdf::ElementPtr orientationRefFrameElem = + _elem->GetElement("orientation_reference_frame"); + orientationRefFrameElem->GetElement("localization")->Set( + this->Localization()); + + sdf::ElementPtr customRPY = + orientationRefFrameElem->GetElement("custom_rpy"); + customRPY->Set(this->CustomRpy()); + customRPY->GetAttribute("parent_frame")->Set( + this->CustomRpyParentFrame()); + + sdf::ElementPtr gravDirX = + orientationRefFrameElem->GetElement("grav_dir_x"); + gravDirX->Set(this->GravityDirX()); + gravDirX->GetAttribute("parent_frame")->Set( + this->GravityDirXParentFrame()); + + sdf::ElementPtr angularVelElem = _elem->GetElement("angular_velocity"); + sdf::ElementPtr angularVelXElem = angularVelElem->GetElement("x"); + sdf::ElementPtr angularVelXNoiseElem = angularVelXElem->GetElement("noise"); + result = result && + this->dataPtr->angularVelXNoise.PopulateElement(angularVelXNoiseElem); + + sdf::ElementPtr angularVelYElem = angularVelElem->GetElement("y"); + sdf::ElementPtr angularVelYNoiseElem = angularVelYElem->GetElement("noise"); + result = result && + this->dataPtr->angularVelYNoise.PopulateElement(angularVelYNoiseElem); + + sdf::ElementPtr angularVelZElem = angularVelElem->GetElement("z"); + sdf::ElementPtr angularVelZNoiseElem = angularVelZElem->GetElement("noise"); + result = result && + this->dataPtr->angularVelZNoise.PopulateElement(angularVelZNoiseElem); + + sdf::ElementPtr linearAccElem = _elem->GetElement("linear_acceleration"); + sdf::ElementPtr linearAccXElem = linearAccElem->GetElement("x"); + sdf::ElementPtr linearAccXNoiseElem = linearAccXElem->GetElement("noise"); + result = result && + this->dataPtr->linearAccelXNoise.PopulateElement(linearAccXNoiseElem); + + sdf::ElementPtr linearAccYElem = linearAccElem->GetElement("y"); + sdf::ElementPtr linearAccYNoiseElem = linearAccYElem->GetElement("noise"); + result = result && + this->dataPtr->linearAccelYNoise.PopulateElement(linearAccYNoiseElem); + + sdf::ElementPtr linearAccZElem = linearAccElem->GetElement("z"); + sdf::ElementPtr linearAccZNoiseElem = linearAccZElem->GetElement("noise"); + result = result && + this->dataPtr->linearAccelZNoise.PopulateElement(linearAccZNoiseElem); + + _elem->GetElement("enable_orientation")->Set( + this->OrientationEnabled()); + + return result; +} diff --git a/src/Lidar.cc b/src/Lidar.cc index e3b54600b..5c33c2f84 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -366,3 +366,35 @@ bool Lidar::operator!=(const Lidar &_lidar) const { return !(*this == _lidar); } + +///////////////////////////////////////////////// +bool Lidar::PopulateElement(sdf::ElementPtr _elem) const +{ + sdf::ElementPtr scanElem = _elem->GetElement("scan"); + sdf::ElementPtr horElem = scanElem->GetElement("horizontal"); + horElem->GetElement("samples")->Set(this->HorizontalScanSamples()); + horElem->GetElement("resolution")->Set( + this->HorizontalScanResolution()); + horElem->GetElement("min_angle")->Set( + this->HorizontalScanMinAngle().Radian()); + horElem->GetElement("max_angle")->Set( + this->HorizontalScanMaxAngle().Radian()); + + sdf::ElementPtr vertElem = scanElem->GetElement("vertical"); + vertElem->GetElement("samples")->Set(this->VerticalScanSamples()); + vertElem->GetElement("resolution")->Set( + this->VerticalScanResolution()); + vertElem->GetElement("min_angle")->Set( + this->VerticalScanMinAngle().Radian()); + vertElem->GetElement("max_angle")->Set( + this->VerticalScanMaxAngle().Radian()); + + sdf::ElementPtr rangeElem = _elem->GetElement("range"); + rangeElem->GetElement("min")->Set(this->RangeMin()); + rangeElem->GetElement("max")->Set(this->RangeMax()); + rangeElem->GetElement("resolution")->Set( + this->RangeResolution()); + + sdf::ElementPtr noiseElem = _elem->GetElement("noise"); + return this->dataPtr->lidarNoise.PopulateElement(noiseElem, true); +} diff --git a/src/Magnetometer.cc b/src/Magnetometer.cc index 796a6898f..130574a16 100644 --- a/src/Magnetometer.cc +++ b/src/Magnetometer.cc @@ -130,3 +130,28 @@ bool Magnetometer::operator==(const Magnetometer &_mag) const return true; } + +///////////////////////////////////////////////// +bool Magnetometer::PopulateElement(sdf::ElementPtr _elem) const +{ + bool result = true; + sdf::ElementPtr magnetometerXElem = _elem->GetElement("x"); + sdf::ElementPtr magnetometerXNoiseElem = + magnetometerXElem->GetElement("noise"); + result = result && + this->dataPtr->noise[0].PopulateElement(magnetometerXNoiseElem); + + sdf::ElementPtr magnetometerYElem = _elem->GetElement("y"); + sdf::ElementPtr magnetometerYNoiseElem = + magnetometerYElem->GetElement("noise"); + result = result && + this->dataPtr->noise[1].PopulateElement(magnetometerYNoiseElem); + + sdf::ElementPtr magnetometerZElem = _elem->GetElement("z"); + sdf::ElementPtr magnetometerZNoiseElem = + magnetometerZElem->GetElement("noise"); + result = result && + this->dataPtr->noise[2].PopulateElement(magnetometerZNoiseElem); + + return result; +} diff --git a/src/Noise.cc b/src/Noise.cc index d509db1e0..69fe54363 100644 --- a/src/Noise.cc +++ b/src/Noise.cc @@ -248,3 +248,45 @@ bool Noise::operator==(const Noise &_noise) const ignition::math::equal(this->dataPtr->dynamicBiasCorrelationTime, _noise.DynamicBiasCorrelationTime()); } + +///////////////////////////////////////////////// +bool Noise::PopulateElement(sdf::ElementPtr _elem, bool _simpleNoise) const +{ + std::string noiseType; + switch (this->Type()) + { + case sdf::NoiseType::NONE: + noiseType = "none"; + break; + case sdf::NoiseType::GAUSSIAN: + noiseType = "gaussian"; + break; + case sdf::NoiseType::GAUSSIAN_QUANTIZED: + noiseType = "gaussian_quantized"; + break; + default: + noiseType = "none"; + } + // camera and lidar does not have type attribute + if (!_simpleNoise) + _elem->GetAttribute("type")->Set(noiseType); + else + _elem->GetElement("type")->Set(noiseType); + + _elem->GetElement("mean")->Set(this->Mean()); + _elem->GetElement("stddev")->Set(this->StdDev()); + + // camera and lidar does not have the sdf params below + if (!_simpleNoise) + { + _elem->GetElement("bias_mean")->Set(this->BiasMean()); + _elem->GetElement("bias_stddev")->Set(this->BiasStdDev()); + _elem->GetElement("dynamic_bias_stddev")->Set( + this->DynamicBiasStdDev()); + _elem->GetElement("dynamic_bias_correlation_time")->Set( + this->DynamicBiasCorrelationTime()); + _elem->GetElement("precision")->Set(this->Precision()); + } + + return true; +} diff --git a/src/Sensor.cc b/src/Sensor.cc index eed455cc6..7943c0655 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -670,3 +670,68 @@ Imu *Sensor::ImuSensor() { return optionalToPointer(this->dataPtr->imu); } + +///////////////////////////////////////////////// +bool Sensor::PopulateElement(sdf::ElementPtr _elem) const +{ + _elem->GetAttribute("type")->Set(this->TypeStr()); + _elem->GetAttribute("name")->Set(this->Name()); + _elem->GetElement("pose")->Set(this->RawPose()); + _elem->GetElement("topic")->Set(this->Topic()); + _elem->GetElement("update_rate")->Set(this->UpdateRate()); + _elem->GetElement("enable_metrics")->Set(this->EnableMetrics()); + + // air pressure + if (this->Type() == sdf::SensorType::AIR_PRESSURE && + this->dataPtr->airPressure) + { + sdf::ElementPtr airPressureElem = _elem->GetElement("air_pressure"); + return this->dataPtr->airPressure->PopulateElement(airPressureElem); + } + // altimeter + else if (this->Type() == sdf::SensorType::ALTIMETER && + this->dataPtr->altimeter) + { + sdf::ElementPtr altimeterElem = _elem->GetElement("altimeter"); + return this->dataPtr->altimeter->PopulateElement(altimeterElem); + } + // camera, depth, thermal, segmentation + else if (this->CameraSensor()) + { + sdf::ElementPtr cameraElem = _elem->GetElement("camera"); + return this->dataPtr->camera->PopulateElement(cameraElem); + } + // force torque + else if (this->Type() == sdf::SensorType::FORCE_TORQUE && + this->dataPtr->forceTorque) + { + sdf::ElementPtr forceTorqueElem = _elem->GetElement("force_torque"); + return this->dataPtr->forceTorque->PopulateElement(forceTorqueElem); + } + // imu + else if (this->Type() == sdf::SensorType::IMU && this->dataPtr->imu) + { + sdf::ElementPtr imuElem = _elem->GetElement("imu"); + return this->dataPtr->imu->PopulateElement(imuElem); + } + // lidar, gpu_lidar + else if ((this->Type() == sdf::SensorType::GPU_LIDAR || + this->Type() == sdf::SensorType::LIDAR) && + this->dataPtr->lidar) + { + sdf::ElementPtr rayElem = (_elem->HasElement("ray")) ? + _elem->GetElement("ray") : _elem->GetElement("lidar"); + return this->dataPtr->lidar->PopulateElement(rayElem); + } + // magnetometer + else if (this->Type() == sdf::SensorType::MAGNETOMETER && + this->dataPtr->magnetometer) + { + sdf::ElementPtr magnetometerElem = _elem->GetElement("magnetometer"); + return this->dataPtr->magnetometer->PopulateElement(magnetometerElem); + } + + std::cout << "Conversion of sensor type: [" << this->TypeStr() << "] from " + << "SDF DOM to Element is not supported yet." << std::endl; + return false; +} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index ddfb34936..bc2fb4481 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -45,6 +45,7 @@ set(tests scene_dom.cc sdf_basic.cc sdf_custom.cc + sdf_dom_conversion.cc surface_dom.cc unknown.cc urdf_gazebo_extensions.cc diff --git a/test/integration/sdf_dom_conversion.cc b/test/integration/sdf_dom_conversion.cc new file mode 100644 index 000000000..78f3e4490 --- /dev/null +++ b/test/integration/sdf_dom_conversion.cc @@ -0,0 +1,404 @@ +/* + * Copyright 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + + +#include + +#include "sdf/AirPressure.hh" +#include "sdf/Altimeter.hh" +#include "sdf/Camera.hh" +#include "sdf/Element.hh" +#include "sdf/Imu.hh" +#include "sdf/Lidar.hh" +#include "sdf/Link.hh" +#include "sdf/Magnetometer.hh" +#include "sdf/Model.hh" +#include "sdf/parser.hh" +#include "sdf/Root.hh" +#include "sdf/Sensor.hh" +#include "test_config.h" + +////////////////////////////////////////////////// +TEST(SDFDomConversion, Sensors) +{ + // this test loads the sensors.sdf test file, then + // 1) converts each type of sensor DOM to Element + // 2) loads the Element back to DOM, + // 3) verify the values + // Most of the verification code is adapted from link_dom.cc + const std::string testFile = sdf::testing::TestFile("sdf", "sensors.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (auto e : errors) + std::cout << e << std::endl; + EXPECT_TRUE(errors.empty()); + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + + // Get the first link + const sdf::Link *link = model->LinkByIndex(0); + ASSERT_NE(nullptr, link); + + // altimeter + { + const sdf::Sensor *sensor = link->SensorByIndex(0); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto altimeterSensor = std::make_unique(); + altimeterSensor->Load(sensorElem); + + // verify values + ASSERT_NE(nullptr, altimeterSensor); + EXPECT_EQ("altimeter_sensor", altimeterSensor->Name()); + EXPECT_EQ(sdf::SensorType::ALTIMETER, altimeterSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d::Zero, altimeterSensor->RawPose()); + EXPECT_FALSE(altimeterSensor->EnableMetrics()); + const sdf::Altimeter *altSensor = altimeterSensor->AltimeterSensor(); + ASSERT_NE(nullptr, altSensor); + EXPECT_DOUBLE_EQ(0.1, altSensor->VerticalPositionNoise().Mean()); + EXPECT_DOUBLE_EQ(0.2, altSensor->VerticalPositionNoise().StdDev()); + EXPECT_DOUBLE_EQ(2.3, altSensor->VerticalVelocityNoise().Mean()); + EXPECT_DOUBLE_EQ(4.5, altSensor->VerticalVelocityNoise().StdDev()); + } + + // camera + { + const sdf::Sensor *sensor = link->SensorByName("camera_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto cameraSensor = std::make_unique(); + cameraSensor->Load(sensorElem); + + ASSERT_NE(nullptr, cameraSensor); + EXPECT_EQ("camera_sensor", cameraSensor->Name()); + EXPECT_EQ(sdf::SensorType::CAMERA, cameraSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), + cameraSensor->RawPose()); + EXPECT_FALSE(cameraSensor->EnableMetrics()); + const sdf::Camera *camSensor = cameraSensor->CameraSensor(); + ASSERT_NE(nullptr, camSensor); + EXPECT_EQ("my_camera", camSensor->Name()); + EXPECT_EQ(ignition::math::Pose3d(0.1, 0.2, 0.3, 0, 0, 0), + camSensor->RawPose()); + EXPECT_DOUBLE_EQ(0.75, camSensor->HorizontalFov().Radian()); + EXPECT_EQ(640u, camSensor->ImageWidth()); + EXPECT_EQ(480u, camSensor->ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::RGB_INT8, camSensor->PixelFormat()); + EXPECT_DOUBLE_EQ(0.2, camSensor->NearClip()); + EXPECT_DOUBLE_EQ(12.3, camSensor->FarClip()); + EXPECT_TRUE(camSensor->SaveFrames()); + EXPECT_EQ("/tmp/cam", camSensor->SaveFramesPath()); + EXPECT_DOUBLE_EQ(0.5, camSensor->ImageNoise().Mean()); + EXPECT_DOUBLE_EQ(0.1, camSensor->ImageNoise().StdDev()); + EXPECT_DOUBLE_EQ(0.1, camSensor->DistortionK1()); + EXPECT_DOUBLE_EQ(0.2, camSensor->DistortionK2()); + EXPECT_DOUBLE_EQ(0.3, camSensor->DistortionK3()); + EXPECT_DOUBLE_EQ(0.4, camSensor->DistortionP1()); + EXPECT_DOUBLE_EQ(0.5, camSensor->DistortionP2()); + EXPECT_EQ(ignition::math::Vector2d(0.2, 0.4), + camSensor->DistortionCenter()); + EXPECT_EQ("custom", camSensor->LensType()); + EXPECT_FALSE(camSensor->LensScaleToHfov()); + EXPECT_DOUBLE_EQ(1.1, camSensor->LensC1()); + EXPECT_DOUBLE_EQ(2.2, camSensor->LensC2()); + EXPECT_DOUBLE_EQ(3.3, camSensor->LensC3()); + EXPECT_DOUBLE_EQ(1.2, camSensor->LensFocalLength()); + EXPECT_EQ("sin", camSensor->LensFunction()); + EXPECT_DOUBLE_EQ(0.7505, camSensor->LensCutoffAngle().Radian()); + EXPECT_EQ(128, camSensor->LensEnvironmentTextureSize()); + EXPECT_DOUBLE_EQ(280, camSensor->LensIntrinsicsFx()); + EXPECT_DOUBLE_EQ(281, camSensor->LensIntrinsicsFy()); + EXPECT_DOUBLE_EQ(162, camSensor->LensIntrinsicsCx()); + EXPECT_DOUBLE_EQ(124, camSensor->LensIntrinsicsCy()); + EXPECT_DOUBLE_EQ(1.2, camSensor->LensIntrinsicsSkew()); + } + + // depth + { + const sdf::Sensor *sensor = link->SensorByName("depth_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto depthSensor = std::make_unique(); + depthSensor->Load(sensorElem); + + ASSERT_NE(nullptr, depthSensor); + EXPECT_EQ("depth_sensor", depthSensor->Name()); + EXPECT_EQ(sdf::SensorType::DEPTH_CAMERA, depthSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0), depthSensor->RawPose()); + EXPECT_TRUE(depthSensor->EnableMetrics()); + const sdf::Camera *depthCamSensor = depthSensor->CameraSensor(); + ASSERT_NE(nullptr, depthCamSensor); + EXPECT_EQ("my_depth_camera", depthCamSensor->Name()); + } + + // rgbd + { + const sdf::Sensor *sensor = link->SensorByName("rgbd_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto rgbdSensor = std::make_unique(); + rgbdSensor->Load(sensorElem); + + ASSERT_NE(nullptr, rgbdSensor); + EXPECT_EQ("rgbd_sensor", rgbdSensor->Name()); + EXPECT_EQ(sdf::SensorType::RGBD_CAMERA, rgbdSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(37, 38, 39, 0, 0, 0), + rgbdSensor->RawPose()); + EXPECT_FALSE(rgbdSensor->EnableMetrics()); + const sdf::Camera *rgbdCamSensor = rgbdSensor->CameraSensor(); + ASSERT_NE(nullptr, rgbdCamSensor); + EXPECT_EQ("my_rgbd_camera", rgbdCamSensor->Name()); + } + + // thermal + { + const sdf::Sensor *sensor = link->SensorByName("thermal_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto thermalSensor = std::make_unique(); + thermalSensor->Load(sensorElem); + + ASSERT_NE(nullptr, thermalSensor); + EXPECT_EQ("thermal_sensor", thermalSensor->Name()); + EXPECT_EQ(sdf::SensorType::THERMAL_CAMERA, thermalSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(37, 38, 39, 0, 0, 0), + thermalSensor->RawPose()); + EXPECT_FALSE(thermalSensor->EnableMetrics()); + const sdf::Camera *thermalCamSensor = thermalSensor->CameraSensor(); + ASSERT_NE(nullptr, thermalCamSensor); + EXPECT_EQ("my_thermal_camera", thermalCamSensor->Name()); + } + + // segmentation + { + const sdf::Sensor *sensor = link->SensorByName("segmentation_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto segmentationSensor = std::make_unique(); + segmentationSensor->Load(sensorElem); + + ASSERT_NE(nullptr, segmentationSensor); + EXPECT_EQ("segmentation_sensor", segmentationSensor->Name()); + EXPECT_EQ(sdf::SensorType::SEGMENTATION_CAMERA, segmentationSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(37, 38, 39, 0, 0, 0), + segmentationSensor->RawPose()); + const sdf::Camera *segmentationCameraSensor = + segmentationSensor->CameraSensor(); + ASSERT_NE(nullptr, segmentationCameraSensor); + EXPECT_EQ("my_segmentation_camera", segmentationCameraSensor->Name()); + EXPECT_TRUE(segmentationCameraSensor->HasSegmentationType()); + EXPECT_EQ("semantic", segmentationCameraSensor->SegmentationType()); + } + + // force torque + { + const sdf::Sensor *sensor = link->SensorByName("force_torque_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto forceTorqueSensor = std::make_unique(); + forceTorqueSensor->Load(sensorElem); + + ASSERT_NE(nullptr, forceTorqueSensor); + EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name()); + EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + forceTorqueSensor->RawPose()); + EXPECT_FALSE(forceTorqueSensor->EnableMetrics()); + } + + // gpu lidar + { + const sdf::Sensor *sensor = link->SensorByName("gpu_lidar_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto gpuLidarSensor = std::make_unique(); + gpuLidarSensor->Load(sensorElem); + + ASSERT_NE(nullptr, gpuLidarSensor); + EXPECT_EQ("gpu_lidar_sensor", gpuLidarSensor->Name()); + EXPECT_EQ(sdf::SensorType::GPU_LIDAR, gpuLidarSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), + gpuLidarSensor->RawPose()); + EXPECT_FALSE(gpuLidarSensor->EnableMetrics()); + const sdf::Lidar *gpuLidar = gpuLidarSensor->LidarSensor(); + ASSERT_NE(nullptr, gpuLidar); + EXPECT_EQ(640u, gpuLidar->HorizontalScanSamples()); + EXPECT_DOUBLE_EQ(1u, gpuLidar->HorizontalScanResolution()); + EXPECT_EQ(ignition::math::Angle(-1.396263), + gpuLidar->HorizontalScanMinAngle()); + EXPECT_EQ(ignition::math::Angle(1.396263), + gpuLidar->HorizontalScanMaxAngle()); + EXPECT_EQ(1u, gpuLidar->VerticalScanSamples()); + EXPECT_DOUBLE_EQ(0.01, gpuLidar->VerticalScanResolution()); + EXPECT_EQ(ignition::math::Angle(0.1), gpuLidar->VerticalScanMinAngle()); + EXPECT_EQ(ignition::math::Angle(0.2), gpuLidar->VerticalScanMaxAngle()); + + EXPECT_DOUBLE_EQ(0.08, gpuLidar->RangeMin()); + EXPECT_DOUBLE_EQ(10.0, gpuLidar->RangeMax()); + EXPECT_DOUBLE_EQ(0.01, gpuLidar->RangeResolution()); + } + + // imu + { + const sdf::Sensor *sensor = link->SensorByName("imu_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto imuSensor = std::make_unique(); + imuSensor->Load(sensorElem); + + ASSERT_NE(nullptr, imuSensor); + EXPECT_EQ("imu_sensor", imuSensor->Name()); + EXPECT_EQ(sdf::SensorType::IMU, imuSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), imuSensor->RawPose()); + EXPECT_FALSE(imuSensor->EnableMetrics()); + const sdf::Imu *imuSensorObj = imuSensor->ImuSensor(); + ASSERT_NE(nullptr, imuSensorObj); + + EXPECT_DOUBLE_EQ(0.0, imuSensorObj->LinearAccelerationXNoise().Mean()); + EXPECT_DOUBLE_EQ(0.1, imuSensorObj->LinearAccelerationXNoise().StdDev()); + EXPECT_DOUBLE_EQ(0.2, + imuSensorObj->LinearAccelerationXNoise().DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(1.0, + imuSensorObj->LinearAccelerationXNoise().DynamicBiasCorrelationTime()); + + EXPECT_DOUBLE_EQ(1.0, imuSensorObj->LinearAccelerationYNoise().Mean()); + EXPECT_DOUBLE_EQ(1.1, imuSensorObj->LinearAccelerationYNoise().StdDev()); + EXPECT_DOUBLE_EQ(1.2, + imuSensorObj->LinearAccelerationYNoise().DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(2.0, + imuSensorObj->LinearAccelerationYNoise().DynamicBiasCorrelationTime()); + + EXPECT_DOUBLE_EQ(2.0, imuSensorObj->LinearAccelerationZNoise().Mean()); + EXPECT_DOUBLE_EQ(2.1, imuSensorObj->LinearAccelerationZNoise().StdDev()); + EXPECT_DOUBLE_EQ(2.2, + imuSensorObj->LinearAccelerationZNoise().DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(3.0, + imuSensorObj->LinearAccelerationZNoise().DynamicBiasCorrelationTime()); + + EXPECT_DOUBLE_EQ(3.0, imuSensorObj->AngularVelocityXNoise().Mean()); + EXPECT_DOUBLE_EQ(3.1, imuSensorObj->AngularVelocityXNoise().StdDev()); + EXPECT_DOUBLE_EQ(4.2, + imuSensorObj->AngularVelocityXNoise().DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(4.0, + imuSensorObj->AngularVelocityXNoise().DynamicBiasCorrelationTime()); + + EXPECT_DOUBLE_EQ(4.0, imuSensorObj->AngularVelocityYNoise().Mean()); + EXPECT_DOUBLE_EQ(4.1, imuSensorObj->AngularVelocityYNoise().StdDev()); + EXPECT_DOUBLE_EQ(5.2, + imuSensorObj->AngularVelocityYNoise().DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(5.0, + imuSensorObj->AngularVelocityYNoise().DynamicBiasCorrelationTime()); + + EXPECT_DOUBLE_EQ(5.0, imuSensorObj->AngularVelocityZNoise().Mean()); + EXPECT_DOUBLE_EQ(5.1, imuSensorObj->AngularVelocityZNoise().StdDev()); + EXPECT_DOUBLE_EQ(6.2, + imuSensorObj->AngularVelocityZNoise().DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(6.0, + imuSensorObj->AngularVelocityZNoise().DynamicBiasCorrelationTime()); + + EXPECT_EQ("ENU", imuSensorObj->Localization()); + EXPECT_EQ("linka", imuSensorObj->CustomRpyParentFrame()); + EXPECT_EQ(ignition::math::Vector3d::UnitY, imuSensorObj->CustomRpy()); + EXPECT_EQ("linkb", imuSensorObj->GravityDirXParentFrame()); + EXPECT_EQ(ignition::math::Vector3d::UnitZ, imuSensorObj->GravityDirX()); + + EXPECT_FALSE(imuSensorObj->OrientationEnabled()); + } + + // magnetometer + { + const sdf::Sensor *sensor = link->SensorByName("magnetometer_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto magnetometerSensor = std::make_unique(); + magnetometerSensor->Load(sensorElem); + + ASSERT_NE(nullptr, magnetometerSensor); + EXPECT_EQ("magnetometer_sensor", magnetometerSensor->Name()); + EXPECT_EQ(sdf::SensorType::MAGNETOMETER, magnetometerSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + magnetometerSensor->RawPose()); + EXPECT_FALSE(magnetometerSensor->EnableMetrics()); + const sdf::Magnetometer *magSensor = + magnetometerSensor->MagnetometerSensor(); + ASSERT_NE(nullptr, magSensor); + EXPECT_DOUBLE_EQ(0.1, magSensor->XNoise().Mean()); + EXPECT_DOUBLE_EQ(0.2, magSensor->XNoise().StdDev()); + EXPECT_DOUBLE_EQ(1.2, magSensor->YNoise().Mean()); + EXPECT_DOUBLE_EQ(2.3, magSensor->YNoise().StdDev()); + EXPECT_DOUBLE_EQ(3.4, magSensor->ZNoise().Mean()); + EXPECT_DOUBLE_EQ(5.6, magSensor->ZNoise().StdDev()); + } + + // air pressure + { + const sdf::Sensor *sensor = link->SensorByName("air_pressure_sensor"); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + + // convert to sdf element and load it back + sensor->PopulateElement(sensorElem); + auto airPressureSensor = std::make_unique(); + airPressureSensor->Load(sensorElem); + + ASSERT_NE(nullptr, airPressureSensor); + EXPECT_EQ("air_pressure_sensor", airPressureSensor->Name()); + EXPECT_EQ(sdf::SensorType::AIR_PRESSURE, airPressureSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 20, 30, 0, 0, 0), + airPressureSensor->RawPose()); + EXPECT_FALSE(airPressureSensor->EnableMetrics()); + const sdf::AirPressure *airSensor = airPressureSensor->AirPressureSensor(); + ASSERT_NE(nullptr, airSensor); + EXPECT_DOUBLE_EQ(3.4, airSensor->PressureNoise().Mean()); + EXPECT_DOUBLE_EQ(5.6, airSensor->PressureNoise().StdDev()); + EXPECT_DOUBLE_EQ(123.4, airSensor->ReferenceAltitude()); + } +} diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index 123441186..c4e6c6744 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -190,6 +190,27 @@ 1 2 3 0 0 0 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 1 + 0.01 + 0.1 + 0.2 + + + + 0.08 + 10.0 + 0.01 + + From f2a286cbf89aca7daa722a70a357e0de32224372 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 17 Nov 2021 00:38:23 -0800 Subject: [PATCH 38/60] Populate light sdf::ElementPtr from Light DOM (#755) * add class to convert sensor dom to elem Signed-off-by: Ian Chen * tweak noise Signed-off-by: Ian Chen * Suggestion to move dom conversion to sensor classes (#754) * Suggestion to move dom conversion to sensor classes Signed-off-by: Nate Koenig * Remove file from smake Signed-off-by: Nate Koenig * remove header file in cmake Signed-off-by: Ian Chen * Fixing build Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Ian Chen * convert light dom to elem Signed-off-by: Ian Chen * remove empty space Signed-off-by: Ian Chen * update api Signed-off-by: Ian Chen * add more tests Signed-off-by: Ian Chen Co-authored-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/Light.hh | 5 + src/Light.cc | 57 +++++++++ src/Light_TEST.cc | 57 +++++++++ test/integration/sdf_dom_conversion.cc | 162 +++++++++++++++++++++++++ test/sdf/lights.sdf | 93 ++++++++++++++ 5 files changed, 374 insertions(+) create mode 100644 test/sdf/lights.sdf diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index 11e5dceb8..281b051f5 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -268,6 +268,11 @@ namespace sdf private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); + /// \brief Create and return an SDF element filled with data from this + /// light object. + /// \return SDF element pointer with updated light values. + public: sdf::ElementPtr ToElement() const; + /// \brief Allow Link::SetPoseRelativeToGraph or World::Load to call /// SetXmlParentName and SetPoseRelativeToGraph, /// but Link::SetPoseRelativeToGraph is a private function, so we need diff --git a/src/Light.cc b/src/Light.cc index 692a925b9..66b266db2 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -18,6 +18,7 @@ #include #include "sdf/Error.hh" #include "sdf/Light.hh" +#include "sdf/parser.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" #include "Utils.hh" @@ -448,3 +449,59 @@ void Light::SetType(const LightType _type) { this->dataPtr->type = _type; } + +///////////////////////////////////////////////// +sdf::ElementPtr Light::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("light.sdf", elem); + + std::string lightTypeStr = "point"; + switch (this->Type()) + { + case LightType::POINT: + lightTypeStr = "point"; + break; + case LightType::DIRECTIONAL: + lightTypeStr = "directional"; + break; + case LightType::SPOT: + lightTypeStr = "spot"; + break; + default: + break; + } + elem->GetAttribute("type")->Set(lightTypeStr); + elem->GetAttribute("name")->Set(this->Name()); + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + elem->GetElement("cast_shadows")->Set(this->CastShadows()); + elem->GetElement("intensity")->Set(this->Intensity()); + elem->GetElement("direction")->Set( + this->Direction()); + elem->GetElement("diffuse")->Set(this->Diffuse()); + elem->GetElement("specular")->Set(this->Specular()); + sdf::ElementPtr attenuationElem = elem->GetElement("attenuation"); + attenuationElem->GetElement("linear")->Set( + this->LinearAttenuationFactor()); + attenuationElem->GetElement("constant")->Set( + this->ConstantAttenuationFactor()); + attenuationElem->GetElement("quadratic")->Set( + this->QuadraticAttenuationFactor()); + attenuationElem->GetElement("range")->Set( + this->AttenuationRange()); + + sdf::ElementPtr spotElem = elem->GetElement("spot"); + spotElem->GetElement("inner_angle")->Set( + this->SpotInnerAngle().Radian()); + spotElem->GetElement("outer_angle")->Set( + this->SpotOuterAngle().Radian()); + spotElem->GetElement("falloff")->Set(this->SpotFalloff()); + return elem; +} diff --git a/src/Light_TEST.cc b/src/Light_TEST.cc index 758cf7955..3ea74177c 100644 --- a/src/Light_TEST.cc +++ b/src/Light_TEST.cc @@ -319,3 +319,60 @@ TEST(DOMLight, AttenuationClamp) light.SetQuadraticAttenuationFactor(-1.0); EXPECT_DOUBLE_EQ(0.0, light.QuadraticAttenuationFactor()); } + +///////////////////////////////////////////////// +TEST(DOMLight, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::Light light; + light.SetName("test_light_assignment"); + light.SetType(sdf::LightType::SPOT); + light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); + light.SetPoseRelativeTo("ground_plane"); + light.SetCastShadows(true); + light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); + light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); + light.SetAttenuationRange(3.2); + light.SetLinearAttenuationFactor(0.1); + light.SetConstantAttenuationFactor(0.5); + light.SetQuadraticAttenuationFactor(0.01); + light.SetDirection({0.1, 0.2, 1}); + light.SetSpotInnerAngle(1.9); + light.SetSpotOuterAngle(3.3); + light.SetSpotFalloff(0.9); + light.SetIntensity(1.7); + + sdf::ElementPtr lightElem = light.ToElement(); + EXPECT_NE(nullptr, lightElem); + EXPECT_EQ(nullptr, light.Element()); + + // verify values after loading the element back + sdf::Light light2; + light2.Load(lightElem); + + EXPECT_NE(nullptr, light2.Element()); + EXPECT_EQ("test_light_assignment", light2.Name()); + EXPECT_EQ(sdf::LightType::SPOT, light2.Type()); + EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); + EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); + EXPECT_TRUE(light2.CastShadows()); + EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); + EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); + EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); + EXPECT_DOUBLE_EQ(0.1, light2.LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.5, light2.ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light2.QuadraticAttenuationFactor()); + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 1), light2.Direction()); + EXPECT_EQ(ignition::math::Angle(1.9), light2.SpotInnerAngle()); + EXPECT_EQ(ignition::math::Angle(3.3), light2.SpotOuterAngle()); + EXPECT_DOUBLE_EQ(0.9, light2.SpotFalloff()); + EXPECT_DOUBLE_EQ(1.7, light2.Intensity()); + + // make changes to DOM and verify ToElement produces updated values + light2.SetCastShadows(false); + sdf::ElementPtr light2Elem = light2.ToElement(); + EXPECT_NE(nullptr, light2Elem); + sdf::Light light3; + light3.Load(light2Elem); + EXPECT_FALSE(light3.CastShadows()); +} diff --git a/test/integration/sdf_dom_conversion.cc b/test/integration/sdf_dom_conversion.cc index 78f3e4490..4ac045898 100644 --- a/test/integration/sdf_dom_conversion.cc +++ b/test/integration/sdf_dom_conversion.cc @@ -24,12 +24,14 @@ #include "sdf/Element.hh" #include "sdf/Imu.hh" #include "sdf/Lidar.hh" +#include "sdf/Light.hh" #include "sdf/Link.hh" #include "sdf/Magnetometer.hh" #include "sdf/Model.hh" #include "sdf/parser.hh" #include "sdf/Root.hh" #include "sdf/Sensor.hh" +#include "sdf/World.hh" #include "test_config.h" ////////////////////////////////////////////////// @@ -402,3 +404,163 @@ TEST(SDFDomConversion, Sensors) EXPECT_DOUBLE_EQ(123.4, airSensor->ReferenceAltitude()); } } + +////////////////////////////////////////////////// +TEST(SDFDomConversion, Lights) +{ + // this test loads the lights.sdf test file, then + // 1) converts Light DOM to Element + // 2) loads the Element back to DOM, + // 3) verify the values + const std::string testFile = sdf::testing::TestFile("sdf", "lights.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (auto e : errors) + std::cout << e << std::endl; + EXPECT_TRUE(errors.empty()); + + // Get the world + const sdf::World *world = root.WorldByIndex(0u); + ASSERT_NE(nullptr, world); + + // Get lights + // point + { + const sdf::Light *light = world->LightByIndex(0); + sdf::ElementPtr lightElem = light->ToElement(); + auto pointLight = std::make_unique(); + pointLight->Load(lightElem); + ASSERT_NE(nullptr, pointLight); + EXPECT_EQ("point_light", pointLight->Name()); + EXPECT_EQ(sdf::LightType::POINT, pointLight->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 2, 100, 0, 0, 0), + pointLight->RawPose()); + EXPECT_FALSE(pointLight->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.0f, 0.1f, 0.8f, 1.0f), + pointLight->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.03f, 0.02f, 0.0f, 0.5f), + pointLight->Specular()); + EXPECT_DOUBLE_EQ(1234.0, pointLight->AttenuationRange()); + // value should be clamped to [0, 1] + EXPECT_DOUBLE_EQ(1.0, pointLight->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, pointLight->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(11.2, pointLight->QuadraticAttenuationFactor()); + } + + // spot + { + const sdf::Light *light = world->LightByIndex(1u); + sdf::ElementPtr lightElem = light->ToElement(); + auto spotLight = std::make_unique(); + spotLight->Load(lightElem); + ASSERT_NE(nullptr, spotLight); + EXPECT_EQ("spot_light", spotLight->Name()); + EXPECT_EQ(sdf::LightType::SPOT, spotLight->Type()); + EXPECT_EQ(ignition::math::Pose3d::Zero, + spotLight->RawPose()); + EXPECT_TRUE(spotLight->CastShadows()); + EXPECT_DOUBLE_EQ(12.34, spotLight->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.01, spotLight->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.02, spotLight->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.0001, spotLight->QuadraticAttenuationFactor()); + EXPECT_EQ(ignition::math::Color(0.0f, 0.05f, 0.06f, 1.0f), + spotLight->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.05f, 0.04f, 0.1f, 1.0f), + spotLight->Specular()); + EXPECT_EQ(ignition::math::Vector3d(1.0, 5.1, 2.1), spotLight->Direction()); + } + + // directional + { + const sdf::Light *light = world->LightByIndex(2u); + sdf::ElementPtr lightElem = light->ToElement(); + auto dirLight = std::make_unique(); + dirLight->Load(lightElem); + ASSERT_NE(nullptr, dirLight); + EXPECT_EQ("directional_light", dirLight->Name()); + EXPECT_EQ(sdf::LightType::DIRECTIONAL, dirLight->Type()); + EXPECT_EQ(ignition::math::Pose3d(1, 1, 2, 0, 0, 0), + dirLight->RawPose()); + EXPECT_TRUE(dirLight->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.0f, 0.0f, 0.3f, 1.0f), + dirLight->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.0f, 0.9f, 0.11f, 1.0f), + dirLight->Specular()); + EXPECT_EQ(ignition::math::Vector3d(5.0, 5.2, 6.0), dirLight->Direction()); + } + + // Get model with lights attached to its link + const sdf::Model *model = world->ModelByName("model_lights"); + ASSERT_NE(nullptr, model); + + // Get the first link + const sdf::Link *link = model->LinkByIndex(0); + ASSERT_NE(nullptr, link); + + // spot + { + const sdf::Light *light = link->LightByName("link_spot_light"); + sdf::ElementPtr lightElem = light->ToElement(); + auto spotLight = std::make_unique(); + spotLight->Load(lightElem); + ASSERT_NE(nullptr, spotLight); + EXPECT_EQ("link_spot_light", spotLight->Name()); + EXPECT_EQ(sdf::LightType::SPOT, spotLight->Type()); + EXPECT_EQ(ignition::math::Pose3d::Zero, + spotLight->RawPose()); + EXPECT_TRUE(spotLight->CastShadows()); + EXPECT_DOUBLE_EQ(12.35, spotLight->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.1, spotLight->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.2, spotLight->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.001, spotLight->QuadraticAttenuationFactor()); + EXPECT_EQ(ignition::math::Color(0.0f, 0.05f, 0.06f, 1.0f), + spotLight->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.03f, 0.02f, 0.0f, 1.0f), + spotLight->Specular()); + EXPECT_EQ(ignition::math::Vector3d(1.0, 5.0, 2.0), spotLight->Direction()); + } + + // point + { + const sdf::Light *light = link->LightByName("link_point_light"); + sdf::ElementPtr lightElem = light->ToElement(); + auto pointLight = std::make_unique(); + pointLight->Load(lightElem); + ASSERT_NE(nullptr, pointLight); + EXPECT_EQ("link_point_light", pointLight->Name()); + EXPECT_EQ(sdf::LightType::POINT, pointLight->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 20, 100, 0, 0, 0), + pointLight->RawPose()); + EXPECT_FALSE(pointLight->CastShadows()); + EXPECT_EQ(ignition::math::Color(1.0f, 0.0f, 0.6f, 1.0f), + pointLight->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.3f, 0.2f, 0.0f, 1.0f), + pointLight->Specular()); + EXPECT_DOUBLE_EQ(1235.0, pointLight->AttenuationRange()); + EXPECT_DOUBLE_EQ(1.0, pointLight->LinearAttenuationFactor()); + // negative value should be clamped to 0 + EXPECT_DOUBLE_EQ(0.0, pointLight->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(10.2, pointLight->QuadraticAttenuationFactor()); + } + + // directional + { + const sdf::Light *light = link->LightByName("link_directional_light"); + sdf::ElementPtr lightElem = light->ToElement(); + auto dirLight = std::make_unique(); + dirLight->Load(lightElem); + ASSERT_NE(nullptr, dirLight); + EXPECT_EQ("link_directional_light", dirLight->Name()); + EXPECT_EQ(sdf::LightType::DIRECTIONAL, dirLight->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, 1, 2, 0, 0, 0), + dirLight->RawPose()); + EXPECT_TRUE(dirLight->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.0f, 1.0f, 0.2f, 1.0f), + dirLight->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.0f, 0.2f, 0.1f, 1.0f), + dirLight->Specular()); + EXPECT_EQ(ignition::math::Vector3d(4.0, 5.0, 6.0), dirLight->Direction()); + } +} diff --git a/test/sdf/lights.sdf b/test/sdf/lights.sdf new file mode 100644 index 000000000..508282f6d --- /dev/null +++ b/test/sdf/lights.sdf @@ -0,0 +1,93 @@ + + + + + + false + 10 2 100 0 0 0 + 0.0 0.1 0.8 1.0 + 0.03 0.02 0.0 0.5 + + 1234 + 2 + 0.01 + 11.2 + + + + + + true + 0.0 0.05 0.06 1.0 + 0.05 0.04 0.1 1.0 + 1 5.1 2.1 + + 12.34 + 0.01 + 0.02 + 0.0001 + + + 0.1 + 0.5 + 2.2 + + + + + true + 1 1 2 0 0 0 + 2.2 + 5 5.2 6 + 0.0 0.0 0.3 1.0 + 0.0 0.9 0.11 1.0 + + + + + + + false + 10 20 100 0 0 0 + 1.0 0.0 0.6 1.0 + 0.3 0.2 0.0 1.0 + + 1235 + 1 + -1.0 + 10.2 + + + + + true + 0.0 0.05 0.06 1.0 + 0.03 0.02 0.0 1.0 + 1 5 2 + + 12.35 + 0.1 + 0.2 + 0.001 + + + 0.1 + 0.5 + 2.2 + + + + + 0 1 2 0 0 0 + 1.2 + 4 5 6 + 0.0 1.0 0.2 1.0 + 0.0 0.2 0.1 1.0 + true + + + + + + + From 8ddf022230de237337ca8a34d86bc78a4e8f9e58 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 22 Nov 2021 12:29:05 -0800 Subject: [PATCH 39/60] Support wide angle camera (#744) Support both "wideanglecamera" and "wide_angle_camera" type strings. Signed-off-by: Ian Chen --- include/sdf/Sensor.hh | 5 ++++- src/Sensor.cc | 11 ++++++++++- src/Sensor_TEST.cc | 6 ++++-- test/integration/link_dom.cc | 33 ++++++++++++++++++++++++++++++++- test/sdf/sensors.sdf | 31 +++++++++++++++++++++++++++++++ 5 files changed, 81 insertions(+), 5 deletions(-) diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index bbde81403..5f6860a8d 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -125,7 +125,10 @@ namespace sdf BOUNDINGBOX_CAMERA = 23, /// \brief A custom sensor - CUSTOM = 24 + CUSTOM = 24, + + /// \brief A wide angle camera sensor + WIDE_ANGLE_CAMERA = 25 }; /// \brief Information about an SDF sensor. diff --git a/src/Sensor.cc b/src/Sensor.cc index 7943c0655..7e6adbb8a 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -64,7 +64,8 @@ const std::vector sensorTypeStrs = "navsat", "segmentation_camera", "boundingbox_camera", - "custom" + "custom", + "wide_angle_camera" }; class sdf::Sensor::Implementation @@ -169,6 +170,7 @@ bool Sensor::operator==(const Sensor &_sensor) const case SensorType::DEPTH_CAMERA: case SensorType::RGBD_CAMERA: case SensorType::THERMAL_CAMERA: + case SensorType::WIDE_ANGLE_CAMERA: case SensorType::SEGMENTATION_CAMERA: case SensorType::BOUNDINGBOX_CAMERA: return *(this->dataPtr->camera) == *(_sensor.dataPtr->camera); @@ -302,6 +304,13 @@ Errors Sensor::Load(ElementPtr _sdf) Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); errors.insert(errors.end(), err.begin(), err.end()); } + else if (type == "wideanglecamera" || type == "wide_angle_camera") + { + this->dataPtr->type = SensorType::WIDE_ANGLE_CAMERA; + this->dataPtr->camera.emplace(); + Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); + errors.insert(errors.end(), err.begin(), err.end()); + } else if (type == "force_torque") { this->dataPtr->type = SensorType::FORCE_TORQUE; diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 28ef3cb77..6881022c9 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -263,7 +263,8 @@ TEST(DOMSensor, Type) sdf::SensorType::WIRELESS_RECEIVER, sdf::SensorType::WIRELESS_TRANSMITTER, sdf::SensorType::THERMAL_CAMERA, - sdf::SensorType::CUSTOM + sdf::SensorType::CUSTOM, + sdf::SensorType::WIDE_ANGLE_CAMERA }; std::vector typeStrs = { @@ -288,7 +289,8 @@ TEST(DOMSensor, Type) "wireless_receiver", "wireless_transmitter", "thermal_camera", - "custom" + "custom", + "wide_angle_camera" }; for (size_t i = 0; i < types.size(); ++i) diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 1c98a1ba2..396c1cd11 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -243,7 +243,7 @@ TEST(DOMLink, Sensors) const sdf::Link *link = model->LinkByIndex(0); ASSERT_NE(nullptr, link); EXPECT_EQ("link", link->Name()); - EXPECT_EQ(25u, link->SensorCount()); + EXPECT_EQ(26u, link->SensorCount()); // Get the altimeter sensor const sdf::Sensor *altimeterSensor = link->SensorByIndex(0); @@ -647,6 +647,37 @@ TEST(DOMLink, Sensors) EXPECT_DOUBLE_EQ(3.4, airSensor->PressureNoise().Mean()); EXPECT_DOUBLE_EQ(5.6, airSensor->PressureNoise().StdDev()); EXPECT_DOUBLE_EQ(123.4, airSensor->ReferenceAltitude()); + + // Get the wide angle camera sensor + EXPECT_TRUE(link->SensorNameExists("wide_angle_camera_sensor")); + const sdf::Sensor *wideAngleCameraSensor = + link->SensorByName("wide_angle_camera_sensor"); + ASSERT_NE(nullptr, wideAngleCameraSensor); + EXPECT_EQ("wide_angle_camera_sensor", wideAngleCameraSensor->Name()); + EXPECT_EQ(sdf::SensorType::WIDE_ANGLE_CAMERA, wideAngleCameraSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(20, 30, 40, 0, 0, 0), + wideAngleCameraSensor->RawPose()); + EXPECT_FALSE(wideAngleCameraSensor->EnableMetrics()); + const sdf::Camera *wideAngleCam = wideAngleCameraSensor->CameraSensor(); + ASSERT_NE(nullptr, wideAngleCam); + EXPECT_EQ("wide_angle_cam", wideAngleCam->Name()); + EXPECT_EQ(ignition::math::Pose3d(0.2, 0.3, 0.4, 0, 0, 0), + wideAngleCam->RawPose()); + EXPECT_DOUBLE_EQ(3.14, wideAngleCam->HorizontalFov().Radian()); + EXPECT_EQ(320u, wideAngleCam->ImageWidth()); + EXPECT_EQ(240u, wideAngleCam->ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::RGB_INT8, wideAngleCam->PixelFormat()); + EXPECT_DOUBLE_EQ(0.1, wideAngleCam->NearClip()); + EXPECT_DOUBLE_EQ(100, wideAngleCam->FarClip()); + EXPECT_EQ("custom", wideAngleCam->LensType()); + EXPECT_TRUE(wideAngleCam->LensScaleToHfov()); + EXPECT_DOUBLE_EQ(1.05, wideAngleCam->LensC1()); + EXPECT_DOUBLE_EQ(4.0, wideAngleCam->LensC2()); + EXPECT_DOUBLE_EQ(0.0, wideAngleCam->LensC3()); + EXPECT_DOUBLE_EQ(1.0, wideAngleCam->LensFocalLength()); + EXPECT_EQ("tan", wideAngleCam->LensFunction()); + EXPECT_DOUBLE_EQ(3.1415, wideAngleCam->LensCutoffAngle().Radian()); + EXPECT_EQ(512, wideAngleCam->LensEnvironmentTextureSize()); } ///////////////////////////////////////////////// diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index c4e6c6744..1c67f2b72 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -633,6 +633,37 @@ + + + 20 30 40 0 0 0 + + 0.2 0.3 0.4 0 0 0 + 3.14 + + 320 + 240 + + + 0.1 + 100 + + + custom + + 1.05 + 4 + 1.0 + tan + + true + 3.1415 + 512 + + + 1 + 30 + wideanglecamera + From cc2d819efc3d345f9fa7ea524c830ce9f80d2957 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 22 Nov 2021 16:33:12 -0800 Subject: [PATCH 40/60] Update Sensor DOM to Element functions and add tests (#757) * update api Signed-off-by: Ian Chen * add more tests Signed-off-by: Ian Chen * noise to elem test Signed-off-by: Ian Chen * remove unused var Signed-off-by: Ian Chen Co-authored-by: Nate Koenig --- include/sdf/AirPressure.hh | 8 ++-- include/sdf/Altimeter.hh | 8 ++-- include/sdf/Camera.hh | 8 ++-- include/sdf/ForceTorque.hh | 8 ++-- include/sdf/Imu.hh | 8 ++-- include/sdf/Lidar.hh | 8 ++-- include/sdf/Magnetometer.hh | 8 ++-- include/sdf/Noise.hh | 11 ++--- include/sdf/Sensor.hh | 8 ++-- src/AirPressure.cc | 14 ++++-- src/AirPressure_TEST.cc | 39 ++++++++++++++++ src/Altimeter.cc | 17 ++++--- src/Altimeter_TEST.cc | 39 ++++++++++++++++ src/Camera.cc | 60 ++++++++++++++++++------ src/Camera_TEST.cc | 45 ++++++++++++++++++ src/ForceTorque.cc | 35 +++++++------- src/ForceTorque_TEST.cc | 50 ++++++++++++++++++++ src/Imu.cc | 34 ++++++-------- src/Imu_TEST.cc | 55 ++++++++++++++++++++++ src/Lidar.cc | 36 ++++++++++++-- src/Lidar_TEST.cc | 52 +++++++++++++++++++++ src/Magnetometer.cc | 24 +++++----- src/Magnetometer_TEST.cc | 39 ++++++++++++++++ src/Noise.cc | 38 +++++++-------- src/Noise_TEST.cc | 40 ++++++++++++++++ src/Sensor.cc | 65 +++++++++++++++----------- src/Sensor_TEST.cc | 43 +++++++++++++++++ test/integration/sdf_dom_conversion.cc | 55 +++++----------------- 28 files changed, 645 insertions(+), 210 deletions(-) diff --git a/include/sdf/AirPressure.hh b/include/sdf/AirPressure.hh index aee47149d..50c3650e4 100644 --- a/include/sdf/AirPressure.hh +++ b/include/sdf/AirPressure.hh @@ -84,10 +84,10 @@ namespace sdf /// \returen True if 'this' != _mag. public: bool operator!=(const AirPressure &_air) const; - /// \brief Fill the provided _elem with data from this sensor type. - /// \param[out] _elem SDF element point to populate - /// \return True if successful. - public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Create and return an SDF element filled with data from this + /// air pressure sensor. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement() const; /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/include/sdf/Altimeter.hh b/include/sdf/Altimeter.hh index 5452da1b0..0394ec3ed 100644 --- a/include/sdf/Altimeter.hh +++ b/include/sdf/Altimeter.hh @@ -76,10 +76,10 @@ namespace sdf /// \returen True if 'this' != _alt. public: bool operator!=(const Altimeter &_alt) const; - /// \brief Fill the provided _elem with data from this sensor type. - /// \param[out] _elem SDF element point to populate - /// \return True if successful. - public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Create and return an SDF element filled with data from this + /// altimeter sensor. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement() const; /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index ae07199dc..b766d4089 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -472,10 +472,10 @@ namespace sdf /// \return True if the camera has instrinsics values set, false otherwise public: bool HasLensIntrinsics() const; - /// \brief Fill the provided _elem with data from this sensor type. - /// \param[out] _elem SDF element point to populate - /// \return True if successful. - public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Create and return an SDF element filled with data from this + /// camera. + /// \return SDF element pointer with updated camera values. + public: sdf::ElementPtr ToElement() const; /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh index b5b9000d4..97fafdee8 100644 --- a/include/sdf/ForceTorque.hh +++ b/include/sdf/ForceTorque.hh @@ -154,10 +154,10 @@ namespace sdf /// \returen True if 'this' != _ft. public: bool operator!=(const ForceTorque &_ft) const; - /// \brief Fill the provided _elem with data from this sensor type. - /// \param[out] _elem SDF element point to populate - /// \return True if successful. - public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Create and return an SDF element filled with data from this + /// force torque sensor. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement() const; /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index 93ac4e91c..f530ffaaa 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -253,10 +253,10 @@ namespace sdf /// \returen True if 'this' != _imu. public: bool operator!=(const Imu &_imu) const; - /// \brief Fill the provided _elem with data from this sensor type. - /// \param[out] _elem SDF element point to populate - /// \return True if successful. - public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Create and return an SDF element filled with data from this + /// imu sensor. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement() const; /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/include/sdf/Lidar.hh b/include/sdf/Lidar.hh index 16d0cbfd5..0beff0a1a 100644 --- a/include/sdf/Lidar.hh +++ b/include/sdf/Lidar.hh @@ -232,10 +232,10 @@ namespace sdf /// \return True if 'this' != _lidar. public: bool operator!=(const Lidar &_lidar) const; - /// \brief Fill the provided _elem with data from this sensor type. - /// \param[out] _elem SDF element point to populate - /// \return True if successful. - public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Create and return an SDF element filled with data from this + /// lidar. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement() const; /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/include/sdf/Magnetometer.hh b/include/sdf/Magnetometer.hh index ed7406c33..213d93207 100644 --- a/include/sdf/Magnetometer.hh +++ b/include/sdf/Magnetometer.hh @@ -85,10 +85,10 @@ namespace sdf /// \returen True if 'this' != _mag. public: bool operator!=(const Magnetometer &_mag) const; - /// \brief Fill the provided _elem with data from this sensor type. - /// \param[out] _elem SDF element point to populate - /// \return True if successful. - public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Create and return an SDF element filled with data from this + /// magnetometer. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement() const; /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/include/sdf/Noise.hh b/include/sdf/Noise.hh index a124a23cb..4c260d1d6 100644 --- a/include/sdf/Noise.hh +++ b/include/sdf/Noise.hh @@ -162,13 +162,10 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Fill the provided _elem with data from this sensor type. - /// \param[out] _elem SDF element point to populate - /// \param[in] _simpleNoise True if the element should be populated with - /// just type, mean, and standard deviation. - /// \return True if successful. - public: bool PopulateElement(sdf::ElementPtr _elem, - bool _simpleNoise = false) const; + /// \brief Create and return an SDF element filled with data from this + /// noise. + /// \return SDF element pointer with updated noise values. + public: sdf::ElementPtr ToElement() const; /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 5f6860a8d..92b0c9cd0 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -206,10 +206,10 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Fill the provided _elem with data from this sensor. - /// \param[out] _elem SDF element point to populate - /// \return True if successful. - public: bool PopulateElement(sdf::ElementPtr _elem) const; + /// \brief Create and return an SDF element filled with data from this + /// sensor. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement() const; /// \brief Get the sensor type. /// \return The sensor type. diff --git a/src/AirPressure.cc b/src/AirPressure.cc index 9ed186200..a8ad9a5e1 100644 --- a/src/AirPressure.cc +++ b/src/AirPressure.cc @@ -17,6 +17,7 @@ #include #include #include "sdf/AirPressure.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -115,11 +116,16 @@ void AirPressure::SetPressureNoise(const Noise &_noise) } ///////////////////////////////////////////////// -bool AirPressure::PopulateElement(sdf::ElementPtr _elem) const +sdf::ElementPtr AirPressure::ToElement() const { - _elem->GetElement("reference_altitude")->Set( + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("air_pressure.sdf", elem); + + elem->GetElement("reference_altitude")->Set( this->ReferenceAltitude()); - sdf::ElementPtr pressureElem = _elem->GetElement("pressure"); + sdf::ElementPtr pressureElem = elem->GetElement("pressure"); sdf::ElementPtr noiseElem = pressureElem->GetElement("noise"); - return this->dataPtr->noise.PopulateElement(noiseElem); + noiseElem->Copy(this->dataPtr->noise.ToElement()); + + return elem; } diff --git a/src/AirPressure_TEST.cc b/src/AirPressure_TEST.cc index b10f8d203..fff895faf 100644 --- a/src/AirPressure_TEST.cc +++ b/src/AirPressure_TEST.cc @@ -92,3 +92,42 @@ TEST(DOMAirPressure, Load) // The AirPressure::Load function is test more thouroughly in the // link_dom.cc integration test. } + +///////////////////////////////////////////////// +TEST(DOMAirPressure, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::AirPressure air; + sdf::Noise noise; + air.SetReferenceAltitude(10.2); + noise.SetType(sdf::NoiseType::GAUSSIAN); + noise.SetMean(1.2); + noise.SetStdDev(2.3); + noise.SetBiasMean(4.5); + noise.SetBiasStdDev(6.7); + noise.SetPrecision(8.9); + air.SetPressureNoise(noise); + + sdf::ElementPtr airElem = air.ToElement(); + EXPECT_NE(nullptr, airElem); + EXPECT_EQ(nullptr, air.Element()); + + // verify values after loading the element back + sdf::AirPressure air2; + air2.Load(airElem); + + EXPECT_DOUBLE_EQ(noise.Mean(), air2.PressureNoise().Mean()); + EXPECT_DOUBLE_EQ(noise.StdDev(), air2.PressureNoise().StdDev()); + EXPECT_DOUBLE_EQ(noise.BiasMean(), air2.PressureNoise().BiasMean()); + EXPECT_DOUBLE_EQ(noise.BiasStdDev(), air2.PressureNoise().BiasStdDev()); + EXPECT_DOUBLE_EQ(noise.Precision(), air2.PressureNoise().Precision()); + EXPECT_DOUBLE_EQ(10.2, air2.ReferenceAltitude()); + + // make changes to DOM and verify ToElement produces updated values + air2.SetReferenceAltitude(111); + sdf::ElementPtr air2Elem = air2.ToElement(); + EXPECT_NE(nullptr, air2Elem); + sdf::AirPressure air3; + air3.Load(air2Elem); + EXPECT_DOUBLE_EQ(111.0, air3.ReferenceAltitude()); +} diff --git a/src/Altimeter.cc b/src/Altimeter.cc index a211a9b81..aa94a48a8 100644 --- a/src/Altimeter.cc +++ b/src/Altimeter.cc @@ -17,6 +17,7 @@ #include #include #include "sdf/Altimeter.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -129,16 +130,18 @@ bool Altimeter::operator==(const Altimeter &_alt) const } ///////////////////////////////////////////////// -bool Altimeter::PopulateElement(sdf::ElementPtr _elem) const +sdf::ElementPtr Altimeter::ToElement() const { - sdf::ElementPtr verticalPosElem = _elem->GetElement("vertical_position"); + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("altimeter.sdf", elem); + + sdf::ElementPtr verticalPosElem = elem->GetElement("vertical_position"); sdf::ElementPtr verticalPosNoiseElem = verticalPosElem->GetElement("noise"); + verticalPosNoiseElem->Copy(this->dataPtr->verticalPositionNoise.ToElement()); - sdf::ElementPtr verticalVelElem = _elem->GetElement("vertical_velocity"); + sdf::ElementPtr verticalVelElem = elem->GetElement("vertical_velocity"); sdf::ElementPtr verticalVelNoiseElem = verticalVelElem->GetElement("noise"); + verticalVelNoiseElem->Copy(this->dataPtr->verticalVelocityNoise.ToElement()); - return - this->dataPtr->verticalPositionNoise.PopulateElement(verticalPosNoiseElem) - && - this->dataPtr->verticalVelocityNoise.PopulateElement(verticalVelNoiseElem); + return elem; } diff --git a/src/Altimeter_TEST.cc b/src/Altimeter_TEST.cc index 981e86970..ad1779772 100644 --- a/src/Altimeter_TEST.cc +++ b/src/Altimeter_TEST.cc @@ -100,3 +100,42 @@ TEST(DOMAltimeter, Load) // The Altimeter::Load function is test more thouroughly in the // link_dom.cc integration test. } + +///////////////////////////////////////////////// +TEST(DOMAltimeter, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::Altimeter alt; + sdf::Noise defaultNoise, noise; + EXPECT_EQ(defaultNoise, alt.VerticalPositionNoise()); + EXPECT_EQ(defaultNoise, alt.VerticalVelocityNoise()); + + noise.SetType(sdf::NoiseType::GAUSSIAN); + noise.SetMean(1.2); + noise.SetStdDev(2.3); + noise.SetBiasMean(4.5); + noise.SetBiasStdDev(6.7); + noise.SetPrecision(8.9); + alt.SetVerticalPositionNoise(noise); + alt.SetVerticalVelocityNoise(noise); + + sdf::ElementPtr altElem = alt.ToElement(); + EXPECT_NE(nullptr, altElem); + EXPECT_EQ(nullptr, alt.Element()); + + // verify values after loading the element back + sdf::Altimeter alt2; + alt2.Load(altElem); + + EXPECT_EQ(noise, alt2.VerticalPositionNoise()); + EXPECT_EQ(noise, alt2.VerticalVelocityNoise()); + + // make changes to DOM and verify ToElement produces updated values + noise.SetMean(2.3); + alt2.SetVerticalPositionNoise(noise); + sdf::ElementPtr alt2Elem = alt2.ToElement(); + EXPECT_NE(nullptr, alt2Elem); + sdf::Altimeter alt3; + alt3.Load(alt2Elem); + EXPECT_EQ(noise, alt3.VerticalPositionNoise()); +} diff --git a/src/Camera.cc b/src/Camera.cc index 0f2bf13d5..52c2705cf 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -16,6 +16,7 @@ */ #include #include "sdf/Camera.hh" +#include "sdf/parser.hh" #include "Utils.hh" using namespace sdf; @@ -985,37 +986,68 @@ bool Camera::HasLensIntrinsics() const } ///////////////////////////////////////////////// -bool Camera::PopulateElement(sdf::ElementPtr _elem) const +sdf::ElementPtr Camera::ToElement() const { - _elem->GetAttribute("name")->Set(this->Name()); - _elem->GetElement("pose")->Set(this->RawPose()); - _elem->GetElement("horizontal_fov")->Set( + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("camera.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + elem->GetElement("horizontal_fov")->Set( this->HorizontalFov().Radian()); - sdf::ElementPtr imageElem = _elem->GetElement("image"); + sdf::ElementPtr imageElem = elem->GetElement("image"); imageElem->GetElement("width")->Set(this->ImageWidth()); imageElem->GetElement("height")->Set(this->ImageHeight()); imageElem->GetElement("format")->Set(this->PixelFormatStr()); - sdf::ElementPtr clipElem = _elem->GetElement("clip"); + sdf::ElementPtr clipElem = elem->GetElement("clip"); clipElem->GetElement("near")->Set(this->NearClip()); clipElem->GetElement("far")->Set(this->FarClip()); if (this->dataPtr->hasDepthCamera) { - sdf::ElementPtr depthElem = _elem->GetElement("depth_camera"); + sdf::ElementPtr depthElem = elem->GetElement("depth_camera"); sdf::ElementPtr depthClipElem = depthElem->GetElement("clip"); depthClipElem->GetElement("near")->Set(this->DepthNearClip()); depthClipElem->GetElement("far")->Set(this->DepthFarClip()); } - sdf::ElementPtr saveElem = _elem->GetElement("save"); + sdf::ElementPtr saveElem = elem->GetElement("save"); saveElem->GetAttribute("enabled")->Set(this->SaveFrames()); if (this->SaveFrames()) saveElem->GetElement("path")->Set(this->SaveFramesPath()); + elem->GetElement("visibility_mask")->Set( + this->VisibilityMask()); + + sdf::ElementPtr noiseElem = elem->GetElement("noise"); + std::string noiseType; + switch (this->dataPtr->imageNoise.Type()) + { + case sdf::NoiseType::NONE: + noiseType = "none"; + break; + case sdf::NoiseType::GAUSSIAN: + noiseType = "gaussian"; + break; + case sdf::NoiseType::GAUSSIAN_QUANTIZED: + noiseType = "gaussian_quantized"; + break; + default: + noiseType = "none"; + } - sdf::ElementPtr noiseElem = _elem->GetElement("noise"); - this->dataPtr->imageNoise.PopulateElement(noiseElem, true); + // camera does not use noise.sdf description + noiseElem->GetElement("type")->Set(noiseType); + noiseElem->GetElement("mean")->Set(this->dataPtr->imageNoise.Mean()); + noiseElem->GetElement("stddev")->Set( + this->dataPtr->imageNoise.StdDev()); - sdf::ElementPtr distortionElem = _elem->GetElement("distortion"); + sdf::ElementPtr distortionElem = elem->GetElement("distortion"); distortionElem->GetElement("k1")->Set(this->DistortionK1()); distortionElem->GetElement("k2")->Set(this->DistortionK2()); distortionElem->GetElement("k3")->Set(this->DistortionK3()); @@ -1024,7 +1056,7 @@ bool Camera::PopulateElement(sdf::ElementPtr _elem) const distortionElem->GetElement("center")->Set( this->DistortionCenter()); - sdf::ElementPtr lensElem = _elem->GetElement("lens"); + sdf::ElementPtr lensElem = elem->GetElement("lens"); lensElem->GetElement("type")->Set(this->LensType()); lensElem->GetElement("scale_to_hfov")->Set(this->LensScaleToHfov()); lensElem->GetElement("cutoff_angle")->Set( @@ -1053,9 +1085,9 @@ bool Camera::PopulateElement(sdf::ElementPtr _elem) const if (this->HasSegmentationType()) { - _elem->GetElement("segmentation_type")->Set( + elem->GetElement("segmentation_type")->Set( this->SegmentationType()); } - return true; + return elem; } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 9487a9692..ff2b74614 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -211,3 +211,48 @@ TEST(DOMCamera, Construction) // The Camera::Load function is tested more thoroughly in the // link_dom.cc integration test. } + +///////////////////////////////////////////////// +TEST(DOMCamera, ToElement) +{ + sdf::Camera cam; + cam.SetName("my_camera"); + EXPECT_EQ("my_camera", cam.Name()); + cam.SetHorizontalFov(1.45); + cam.SetImageWidth(123); + cam.SetImageHeight(125); + cam.SetPixelFormat(sdf::PixelFormatType::L_INT8); + cam.SetNearClip(0.2); + cam.SetFarClip(200.2); + cam.SetVisibilityMask(123u); + cam.SetPoseRelativeTo("/frame"); + cam.SetSaveFrames(true); + cam.SetSaveFramesPath("/tmp"); + + sdf::ElementPtr camElem = cam.ToElement(); + EXPECT_NE(nullptr, camElem); + EXPECT_EQ(nullptr, cam.Element()); + + // verify values after loading the element back + sdf::Camera cam2; + cam2.Load(camElem); + + EXPECT_DOUBLE_EQ(1.45, cam2.HorizontalFov().Radian()); + EXPECT_EQ(123u, cam2.ImageWidth()); + EXPECT_EQ(125u, cam2.ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::L_INT8 , cam2.PixelFormat()); + EXPECT_DOUBLE_EQ(0.2, cam2.NearClip()); + EXPECT_DOUBLE_EQ(200.2, cam2.FarClip()); + EXPECT_EQ(123u, cam2.VisibilityMask()); + EXPECT_EQ("/frame", cam2.PoseRelativeTo()); + EXPECT_TRUE(cam2.SaveFrames()); + EXPECT_EQ("/tmp", cam2.SaveFramesPath()); + + // make changes to DOM and verify ToElement produces updated values + cam2.SetNearClip(0.33); + sdf::ElementPtr cam2Elem = cam2.ToElement(); + EXPECT_NE(nullptr, cam2Elem); + sdf::Camera cam3; + cam3.Load(cam2Elem); + EXPECT_DOUBLE_EQ(0.33, cam3.NearClip()); +} diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc index dea4926e4..4fe72bab5 100644 --- a/src/ForceTorque.cc +++ b/src/ForceTorque.cc @@ -16,6 +16,7 @@ */ #include #include "sdf/ForceTorque.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -279,9 +280,11 @@ void ForceTorque::SetMeasureDirection( } ///////////////////////////////////////////////// -bool ForceTorque::PopulateElement(sdf::ElementPtr _elem) const +sdf::ElementPtr ForceTorque::ToElement() const { - bool result = true; + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("forcetorque.sdf", elem); + std::string frame; switch (this->Frame()) { @@ -299,7 +302,7 @@ bool ForceTorque::PopulateElement(sdf::ElementPtr _elem) const break; } if (!frame.empty()) - _elem->GetElement("frame")->Set(frame); + elem->GetElement("frame")->Set(frame); std::string measureDirection; switch (this->MeasureDirection()) @@ -316,40 +319,34 @@ bool ForceTorque::PopulateElement(sdf::ElementPtr _elem) const } if (!measureDirection.empty()) { - _elem->GetElement("measure_direction")->Set(measureDirection); + elem->GetElement("measure_direction")->Set(measureDirection); } - sdf::ElementPtr forceElem = _elem->GetElement("force"); + sdf::ElementPtr forceElem = elem->GetElement("force"); sdf::ElementPtr forceXElem = forceElem->GetElement("x"); sdf::ElementPtr forceXNoiseElem = forceXElem->GetElement("noise"); - result = result && - this->dataPtr->forceXNoise.PopulateElement(forceXNoiseElem); + forceXNoiseElem->Copy(this->dataPtr->forceXNoise.ToElement()); sdf::ElementPtr forceYElem = forceElem->GetElement("y"); sdf::ElementPtr forceYNoiseElem = forceYElem->GetElement("noise"); - result = result && - this->dataPtr->forceYNoise.PopulateElement(forceYNoiseElem); + forceYNoiseElem->Copy(this->dataPtr->forceYNoise.ToElement()); sdf::ElementPtr forceZElem = forceElem->GetElement("z"); sdf::ElementPtr forceZNoiseElem = forceZElem->GetElement("noise"); - result = result && - this->dataPtr->forceZNoise.PopulateElement(forceZNoiseElem); + forceZNoiseElem->Copy(this->dataPtr->forceZNoise.ToElement()); - sdf::ElementPtr torqueElem = _elem->GetElement("torque"); + sdf::ElementPtr torqueElem = elem->GetElement("torque"); sdf::ElementPtr torqueXElem = torqueElem->GetElement("x"); sdf::ElementPtr torqueXNoiseElem = torqueXElem->GetElement("noise"); - result = result && - this->dataPtr->torqueXNoise.PopulateElement(torqueXNoiseElem); + torqueXNoiseElem->Copy(this->dataPtr->torqueXNoise.ToElement()); sdf::ElementPtr torqueYElem = torqueElem->GetElement("y"); sdf::ElementPtr torqueYNoiseElem = torqueYElem->GetElement("noise"); - result = result && - this->dataPtr->torqueYNoise.PopulateElement(torqueYNoiseElem); + torqueYNoiseElem->Copy(this->dataPtr->torqueYNoise.ToElement()); sdf::ElementPtr torqueZElem = torqueElem->GetElement("z"); sdf::ElementPtr torqueZNoiseElem = torqueZElem->GetElement("noise"); - result = result && - this->dataPtr->torqueZNoise.PopulateElement(torqueZNoiseElem); + torqueZNoiseElem->Copy(this->dataPtr->torqueZNoise.ToElement()); - return result; + return elem; } diff --git a/src/ForceTorque_TEST.cc b/src/ForceTorque_TEST.cc index 6acba8514..6a0675689 100644 --- a/src/ForceTorque_TEST.cc +++ b/src/ForceTorque_TEST.cc @@ -109,3 +109,53 @@ TEST(DOMForceTorque, Load) // The ForceTorque::Load function is tested more thouroughly in the // link_dom.cc integration test. } + +///////////////////////////////////////////////// +TEST(DOMForceTorque, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::ForceTorque ft; + sdf::Noise noise; + + noise.SetType(sdf::NoiseType::GAUSSIAN); + noise.SetMean(1.2); + noise.SetStdDev(2.3); + noise.SetBiasMean(4.5); + noise.SetBiasStdDev(6.7); + noise.SetPrecision(8.9); + ft.SetForceXNoise(noise); + ft.SetForceYNoise(noise); + ft.SetForceZNoise(noise); + ft.SetTorqueXNoise(noise); + ft.SetTorqueYNoise(noise); + ft.SetTorqueZNoise(noise); + ft.SetFrame(sdf::ForceTorqueFrame::PARENT); + ft.SetMeasureDirection(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD); + + sdf::ElementPtr ftElem = ft.ToElement(); + EXPECT_NE(nullptr, ftElem); + EXPECT_EQ(nullptr, ft.Element()); + + // verify values after loading the element back + sdf::ForceTorque ft2; + ft2.Load(ftElem); + + EXPECT_EQ(noise, ft2.ForceXNoise()); + EXPECT_EQ(noise, ft2.ForceYNoise()); + EXPECT_EQ(noise, ft2.ForceZNoise()); + EXPECT_EQ(noise, ft2.TorqueXNoise()); + EXPECT_EQ(noise, ft2.TorqueYNoise()); + EXPECT_EQ(noise, ft2.TorqueZNoise()); + EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, ft2.Frame()); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, + ft2.MeasureDirection()); + + // make changes to DOM and verify ToElement produces updated values + ft2.SetMeasureDirection(sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT); + sdf::ElementPtr ft2Elem = ft2.ToElement(); + EXPECT_NE(nullptr, ft2Elem); + sdf::ForceTorque ft3; + ft3.Load(ft2Elem); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT, + ft3.MeasureDirection()); +} diff --git a/src/Imu.cc b/src/Imu.cc index a51a39cc0..54b6bcf86 100644 --- a/src/Imu.cc +++ b/src/Imu.cc @@ -17,6 +17,7 @@ #include #include #include "sdf/Imu.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -367,12 +368,13 @@ bool Imu::OrientationEnabled() const } ///////////////////////////////////////////////// -bool Imu::PopulateElement(sdf::ElementPtr _elem) const +sdf::ElementPtr Imu::ToElement() const { - bool result = true; + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("imu.sdf", elem); sdf::ElementPtr orientationRefFrameElem = - _elem->GetElement("orientation_reference_frame"); + elem->GetElement("orientation_reference_frame"); orientationRefFrameElem->GetElement("localization")->Set( this->Localization()); @@ -388,40 +390,34 @@ bool Imu::PopulateElement(sdf::ElementPtr _elem) const gravDirX->GetAttribute("parent_frame")->Set( this->GravityDirXParentFrame()); - sdf::ElementPtr angularVelElem = _elem->GetElement("angular_velocity"); + sdf::ElementPtr angularVelElem = elem->GetElement("angular_velocity"); sdf::ElementPtr angularVelXElem = angularVelElem->GetElement("x"); sdf::ElementPtr angularVelXNoiseElem = angularVelXElem->GetElement("noise"); - result = result && - this->dataPtr->angularVelXNoise.PopulateElement(angularVelXNoiseElem); + angularVelXNoiseElem->Copy(this->dataPtr->angularVelXNoise.ToElement()); sdf::ElementPtr angularVelYElem = angularVelElem->GetElement("y"); sdf::ElementPtr angularVelYNoiseElem = angularVelYElem->GetElement("noise"); - result = result && - this->dataPtr->angularVelYNoise.PopulateElement(angularVelYNoiseElem); + angularVelYNoiseElem->Copy(this->dataPtr->angularVelYNoise.ToElement()); sdf::ElementPtr angularVelZElem = angularVelElem->GetElement("z"); sdf::ElementPtr angularVelZNoiseElem = angularVelZElem->GetElement("noise"); - result = result && - this->dataPtr->angularVelZNoise.PopulateElement(angularVelZNoiseElem); + angularVelZNoiseElem->Copy(this->dataPtr->angularVelZNoise.ToElement()); - sdf::ElementPtr linearAccElem = _elem->GetElement("linear_acceleration"); + sdf::ElementPtr linearAccElem = elem->GetElement("linear_acceleration"); sdf::ElementPtr linearAccXElem = linearAccElem->GetElement("x"); sdf::ElementPtr linearAccXNoiseElem = linearAccXElem->GetElement("noise"); - result = result && - this->dataPtr->linearAccelXNoise.PopulateElement(linearAccXNoiseElem); + linearAccXNoiseElem->Copy(this->dataPtr->linearAccelXNoise.ToElement()); sdf::ElementPtr linearAccYElem = linearAccElem->GetElement("y"); sdf::ElementPtr linearAccYNoiseElem = linearAccYElem->GetElement("noise"); - result = result && - this->dataPtr->linearAccelYNoise.PopulateElement(linearAccYNoiseElem); + linearAccYNoiseElem->Copy(this->dataPtr->linearAccelYNoise.ToElement()); sdf::ElementPtr linearAccZElem = linearAccElem->GetElement("z"); sdf::ElementPtr linearAccZNoiseElem = linearAccZElem->GetElement("noise"); - result = result && - this->dataPtr->linearAccelZNoise.PopulateElement(linearAccZNoiseElem); + linearAccZNoiseElem->Copy(this->dataPtr->linearAccelZNoise.ToElement()); - _elem->GetElement("enable_orientation")->Set( + elem->GetElement("enable_orientation")->Set( this->OrientationEnabled()); - return result; + return elem; } diff --git a/src/Imu_TEST.cc b/src/Imu_TEST.cc index 4deda538b..83b979f24 100644 --- a/src/Imu_TEST.cc +++ b/src/Imu_TEST.cc @@ -126,3 +126,58 @@ TEST(DOMImu, Load) // The Imu::Load function is test more thouroughly in the // link_dom.cc integration test. } + +///////////////////////////////////////////////// +TEST(DOMImu, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::Imu imu; + sdf::Noise noise; + noise.SetType(sdf::NoiseType::GAUSSIAN); + noise.SetMean(1.2); + noise.SetStdDev(2.3); + noise.SetBiasMean(4.5); + noise.SetBiasStdDev(6.7); + noise.SetPrecision(8.9); + imu.SetLinearAccelerationXNoise(noise); + imu.SetLinearAccelerationYNoise(noise); + imu.SetLinearAccelerationZNoise(noise); + imu.SetAngularVelocityXNoise(noise); + imu.SetAngularVelocityYNoise(noise); + imu.SetAngularVelocityZNoise(noise); + imu.SetGravityDirX(ignition::math::Vector3d::Zero); + imu.SetGravityDirXParentFrame("my_frame"); + imu.SetCustomRpy(ignition::math::Vector3d::UnitZ); + imu.SetCustomRpyParentFrame("other_frame"); + imu.SetLocalization("NED"); + imu.SetOrientationEnabled(false); + + sdf::ElementPtr imuElem = imu.ToElement(); + EXPECT_NE(nullptr, imuElem); + EXPECT_EQ(nullptr, imu.Element()); + + // verify values after loading the element back + sdf::Imu imu2; + imu2.Load(imuElem); + + EXPECT_EQ(noise, imu2.LinearAccelerationXNoise()); + EXPECT_EQ(noise, imu2.LinearAccelerationYNoise()); + EXPECT_EQ(noise, imu2.LinearAccelerationZNoise()); + EXPECT_EQ(noise, imu2.AngularVelocityXNoise()); + EXPECT_EQ(noise, imu2.AngularVelocityYNoise()); + EXPECT_EQ(noise, imu2.AngularVelocityZNoise()); + EXPECT_EQ(ignition::math::Vector3d::Zero, imu2.GravityDirX()); + EXPECT_EQ("my_frame", imu2.GravityDirXParentFrame()); + EXPECT_EQ(ignition::math::Vector3d::UnitZ, imu2.CustomRpy()); + EXPECT_EQ("other_frame", imu2.CustomRpyParentFrame()); + EXPECT_EQ("NED", imu2.Localization()); + EXPECT_FALSE(imu2.OrientationEnabled()); + + // make changes to DOM and verify ToElement produces updated values + imu2.SetGravityDirX(ignition::math::Vector3d(1, 2, 3));; + sdf::ElementPtr imu2Elem = imu2.ToElement(); + EXPECT_NE(nullptr, imu2Elem); + sdf::Imu imu3; + imu3.Load(imu2Elem); + EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), imu3.GravityDirX()); +} diff --git a/src/Lidar.cc b/src/Lidar.cc index 5c33c2f84..e97915df5 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -15,6 +15,7 @@ * */ #include "sdf/Lidar.hh" +#include "sdf/parser.hh" using namespace sdf; using namespace ignition; @@ -368,9 +369,12 @@ bool Lidar::operator!=(const Lidar &_lidar) const } ///////////////////////////////////////////////// -bool Lidar::PopulateElement(sdf::ElementPtr _elem) const +sdf::ElementPtr Lidar::ToElement() const { - sdf::ElementPtr scanElem = _elem->GetElement("scan"); + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("lidar.sdf", elem); + + sdf::ElementPtr scanElem = elem->GetElement("scan"); sdf::ElementPtr horElem = scanElem->GetElement("horizontal"); horElem->GetElement("samples")->Set(this->HorizontalScanSamples()); horElem->GetElement("resolution")->Set( @@ -389,12 +393,34 @@ bool Lidar::PopulateElement(sdf::ElementPtr _elem) const vertElem->GetElement("max_angle")->Set( this->VerticalScanMaxAngle().Radian()); - sdf::ElementPtr rangeElem = _elem->GetElement("range"); + sdf::ElementPtr rangeElem = elem->GetElement("range"); rangeElem->GetElement("min")->Set(this->RangeMin()); rangeElem->GetElement("max")->Set(this->RangeMax()); rangeElem->GetElement("resolution")->Set( this->RangeResolution()); - sdf::ElementPtr noiseElem = _elem->GetElement("noise"); - return this->dataPtr->lidarNoise.PopulateElement(noiseElem, true); + sdf::ElementPtr noiseElem = elem->GetElement("noise"); + std::string noiseType; + switch (this->dataPtr->lidarNoise.Type()) + { + case sdf::NoiseType::NONE: + noiseType = "none"; + break; + case sdf::NoiseType::GAUSSIAN: + noiseType = "gaussian"; + break; + case sdf::NoiseType::GAUSSIAN_QUANTIZED: + noiseType = "gaussian_quantized"; + break; + default: + noiseType = "none"; + } + + // lidar does not use noise.sdf description + noiseElem->GetElement("type")->Set(noiseType); + noiseElem->GetElement("mean")->Set(this->dataPtr->lidarNoise.Mean()); + noiseElem->GetElement("stddev")->Set( + this->dataPtr->lidarNoise.StdDev()); + + return elem; } diff --git a/src/Lidar_TEST.cc b/src/Lidar_TEST.cc index 8dea4494d..2a382db31 100644 --- a/src/Lidar_TEST.cc +++ b/src/Lidar_TEST.cc @@ -118,3 +118,55 @@ TEST(DOMLidar, Load) // The Lidar::Load function is tested more thouroughly in the // link_dom.cc integration test. } + +///////////////////////////////////////////////// +TEST(DOMLidar, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::Lidar lidar; + lidar.SetHorizontalScanSamples(123); + lidar.SetHorizontalScanResolution(0.45); + lidar.SetHorizontalScanMinAngle(ignition::math::Angle(0.67)); + lidar.SetHorizontalScanMaxAngle(ignition::math::Angle(0.89)); + lidar.SetVerticalScanSamples(98); + lidar.SetVerticalScanResolution(0.76); + lidar.SetVerticalScanMinAngle(ignition::math::Angle(0.54)); + lidar.SetVerticalScanMaxAngle(ignition::math::Angle(0.321)); + lidar.SetRangeMin(1.2); + lidar.SetRangeMax(3.4); + lidar.SetRangeResolution(5.6); + + sdf::Noise noise; + noise.SetMean(6.5); + noise.SetStdDev(3.79); + lidar.SetLidarNoise(noise); + + sdf::ElementPtr lidarElem = lidar.ToElement(); + EXPECT_NE(nullptr, lidarElem); + EXPECT_EQ(nullptr, lidar.Element()); + + // verify values after loading the element back + sdf::Lidar lidar2; + lidar2.Load(lidarElem); + + EXPECT_EQ(123u, lidar2.HorizontalScanSamples()); + EXPECT_DOUBLE_EQ(0.45, lidar2.HorizontalScanResolution()); + EXPECT_DOUBLE_EQ(0.67, *(lidar2.HorizontalScanMinAngle())); + EXPECT_DOUBLE_EQ(0.89, *(lidar2.HorizontalScanMaxAngle())); + EXPECT_EQ(98u, lidar2.VerticalScanSamples()); + EXPECT_DOUBLE_EQ(0.76, lidar2.VerticalScanResolution()); + EXPECT_DOUBLE_EQ(0.54, *(lidar2.VerticalScanMinAngle())); + EXPECT_DOUBLE_EQ(0.321, *(lidar2.VerticalScanMaxAngle())); + EXPECT_DOUBLE_EQ(1.2, lidar2.RangeMin()); + EXPECT_DOUBLE_EQ(3.4, lidar2.RangeMax()); + EXPECT_DOUBLE_EQ(5.6, lidar2.RangeResolution()); + EXPECT_EQ(noise, lidar2.LidarNoise()); + + // make changes to DOM and verify ToElement produces updated values + lidar2.SetHorizontalScanSamples(111u); + sdf::ElementPtr lidar2Elem = lidar2.ToElement(); + EXPECT_NE(nullptr, lidar2Elem); + sdf::Lidar lidar3; + lidar3.Load(lidar2Elem); + EXPECT_EQ(111u, lidar3.HorizontalScanSamples()); +} diff --git a/src/Magnetometer.cc b/src/Magnetometer.cc index 130574a16..4e8cafc0e 100644 --- a/src/Magnetometer.cc +++ b/src/Magnetometer.cc @@ -17,6 +17,7 @@ #include #include #include "sdf/Magnetometer.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -132,26 +133,25 @@ bool Magnetometer::operator==(const Magnetometer &_mag) const } ///////////////////////////////////////////////// -bool Magnetometer::PopulateElement(sdf::ElementPtr _elem) const +sdf::ElementPtr Magnetometer::ToElement() const { - bool result = true; - sdf::ElementPtr magnetometerXElem = _elem->GetElement("x"); + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("magnetometer.sdf", elem); + + sdf::ElementPtr magnetometerXElem = elem->GetElement("x"); sdf::ElementPtr magnetometerXNoiseElem = magnetometerXElem->GetElement("noise"); - result = result && - this->dataPtr->noise[0].PopulateElement(magnetometerXNoiseElem); + magnetometerXNoiseElem->Copy(this->dataPtr->noise[0].ToElement()); - sdf::ElementPtr magnetometerYElem = _elem->GetElement("y"); + sdf::ElementPtr magnetometerYElem = elem->GetElement("y"); sdf::ElementPtr magnetometerYNoiseElem = magnetometerYElem->GetElement("noise"); - result = result && - this->dataPtr->noise[1].PopulateElement(magnetometerYNoiseElem); + magnetometerYNoiseElem->Copy(this->dataPtr->noise[1].ToElement()); - sdf::ElementPtr magnetometerZElem = _elem->GetElement("z"); + sdf::ElementPtr magnetometerZElem = elem->GetElement("z"); sdf::ElementPtr magnetometerZNoiseElem = magnetometerZElem->GetElement("noise"); - result = result && - this->dataPtr->noise[2].PopulateElement(magnetometerZNoiseElem); + magnetometerZNoiseElem->Copy(this->dataPtr->noise[2].ToElement()); - return result; + return elem; } diff --git a/src/Magnetometer_TEST.cc b/src/Magnetometer_TEST.cc index e7b45dc09..11676cf98 100644 --- a/src/Magnetometer_TEST.cc +++ b/src/Magnetometer_TEST.cc @@ -111,3 +111,42 @@ TEST(DOMMagnetometer, Load) // The Magnetometer::Load function is test more thouroughly in the // link_dom.cc integration test. } + +///////////////////////////////////////////////// +TEST(DOMMagnetometer, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::Magnetometer mag; + sdf::Noise noise; + noise.SetType(sdf::NoiseType::GAUSSIAN); + noise.SetMean(1.2); + noise.SetStdDev(2.3); + noise.SetBiasMean(4.5); + noise.SetBiasStdDev(6.7); + noise.SetPrecision(8.9); + mag.SetXNoise(noise); + mag.SetYNoise(noise); + mag.SetZNoise(noise); + + sdf::ElementPtr magElem = mag.ToElement(); + EXPECT_NE(nullptr, magElem); + EXPECT_EQ(nullptr, mag.Element()); + + // verify values after loading the element back + sdf::Magnetometer mag2; + mag2.Load(magElem); + + EXPECT_EQ(noise, mag2.XNoise()); + EXPECT_EQ(noise, mag2.YNoise()); + EXPECT_EQ(noise, mag2.ZNoise()); + + // make changes to DOM and verify ToElement produces updated values + noise.SetBiasMean(11.22); + mag2.SetXNoise(noise); + sdf::ElementPtr mag2Elem = mag2.ToElement(); + EXPECT_NE(nullptr, mag2Elem); + sdf::Magnetometer mag3; + mag3.Load(mag2Elem); + EXPECT_EQ(noise, mag3.XNoise()); + EXPECT_NE(noise, mag.XNoise()); +} diff --git a/src/Noise.cc b/src/Noise.cc index 69fe54363..c59283e3a 100644 --- a/src/Noise.cc +++ b/src/Noise.cc @@ -17,6 +17,7 @@ #include #include "sdf/Noise.hh" +#include "sdf/parser.hh" #include "sdf/Types.hh" using namespace sdf; @@ -250,8 +251,11 @@ bool Noise::operator==(const Noise &_noise) const } ///////////////////////////////////////////////// -bool Noise::PopulateElement(sdf::ElementPtr _elem, bool _simpleNoise) const +sdf::ElementPtr Noise::ToElement() const { + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("noise.sdf", elem); + std::string noiseType; switch (this->Type()) { @@ -267,26 +271,18 @@ bool Noise::PopulateElement(sdf::ElementPtr _elem, bool _simpleNoise) const default: noiseType = "none"; } - // camera and lidar does not have type attribute - if (!_simpleNoise) - _elem->GetAttribute("type")->Set(noiseType); - else - _elem->GetElement("type")->Set(noiseType); - - _elem->GetElement("mean")->Set(this->Mean()); - _elem->GetElement("stddev")->Set(this->StdDev()); + elem->GetAttribute("type")->Set(noiseType); + elem->GetElement("mean")->Set(this->Mean()); + elem->GetElement("stddev")->Set(this->StdDev()); // camera and lidar does not have the sdf params below - if (!_simpleNoise) - { - _elem->GetElement("bias_mean")->Set(this->BiasMean()); - _elem->GetElement("bias_stddev")->Set(this->BiasStdDev()); - _elem->GetElement("dynamic_bias_stddev")->Set( - this->DynamicBiasStdDev()); - _elem->GetElement("dynamic_bias_correlation_time")->Set( - this->DynamicBiasCorrelationTime()); - _elem->GetElement("precision")->Set(this->Precision()); - } - - return true; + elem->GetElement("bias_mean")->Set(this->BiasMean()); + elem->GetElement("bias_stddev")->Set(this->BiasStdDev()); + elem->GetElement("dynamic_bias_stddev")->Set( + this->DynamicBiasStdDev()); + elem->GetElement("dynamic_bias_correlation_time")->Set( + this->DynamicBiasCorrelationTime()); + elem->GetElement("precision")->Set(this->Precision()); + + return elem; } diff --git a/src/Noise_TEST.cc b/src/Noise_TEST.cc index 2c1418966..06e0b88ac 100644 --- a/src/Noise_TEST.cc +++ b/src/Noise_TEST.cc @@ -219,3 +219,43 @@ TEST(DOMNoise, Load) EXPECT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(10.2, noise.DynamicBiasCorrelationTime()); } + +///////////////////////////////////////////////// +TEST(DOMNoise, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::Noise noise; + noise.SetType(sdf::NoiseType::GAUSSIAN); + noise.SetMean(1.2); + noise.SetStdDev(2.3); + noise.SetBiasMean(4.5); + noise.SetBiasStdDev(6.7); + noise.SetPrecision(8.9); + noise.SetDynamicBiasStdDev(9.1); + noise.SetDynamicBiasCorrelationTime(19.12); + + sdf::ElementPtr noiseElem = noise.ToElement(); + EXPECT_NE(nullptr, noiseElem); + EXPECT_EQ(nullptr, noise.Element()); + + // verify values after loading the element back + sdf::Noise noise2; + noise2.Load(noiseElem); + + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, noise2.Type()); + EXPECT_DOUBLE_EQ(1.2, noise2.Mean()); + EXPECT_DOUBLE_EQ(2.3, noise2.StdDev()); + EXPECT_DOUBLE_EQ(4.5, noise2.BiasMean()); + EXPECT_DOUBLE_EQ(6.7, noise2.BiasStdDev()); + EXPECT_DOUBLE_EQ(8.9, noise2.Precision()); + EXPECT_DOUBLE_EQ(9.1, noise2.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(19.12, noise2.DynamicBiasCorrelationTime()); + + // make changes to DOM and verify ToElement produces updated values + noise2.SetPrecision(0.1234); + sdf::ElementPtr noise2Elem = noise2.ToElement(); + EXPECT_NE(nullptr, noise2Elem); + sdf::Noise noise3; + noise3.Load(noise2Elem); + EXPECT_DOUBLE_EQ(0.1234, noise3.Precision()); +} diff --git a/src/Sensor.cc b/src/Sensor.cc index 7e6adbb8a..264592aa1 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -28,6 +28,7 @@ #include "sdf/Imu.hh" #include "sdf/Magnetometer.hh" #include "sdf/Lidar.hh" +#include "sdf/parser.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" @@ -681,66 +682,78 @@ Imu *Sensor::ImuSensor() } ///////////////////////////////////////////////// -bool Sensor::PopulateElement(sdf::ElementPtr _elem) const +sdf::ElementPtr Sensor::ToElement() const { - _elem->GetAttribute("type")->Set(this->TypeStr()); - _elem->GetAttribute("name")->Set(this->Name()); - _elem->GetElement("pose")->Set(this->RawPose()); - _elem->GetElement("topic")->Set(this->Topic()); - _elem->GetElement("update_rate")->Set(this->UpdateRate()); - _elem->GetElement("enable_metrics")->Set(this->EnableMetrics()); + 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"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + elem->GetElement("topic")->Set(this->Topic()); + elem->GetElement("update_rate")->Set(this->UpdateRate()); + elem->GetElement("enable_metrics")->Set(this->EnableMetrics()); // air pressure if (this->Type() == sdf::SensorType::AIR_PRESSURE && this->dataPtr->airPressure) { - sdf::ElementPtr airPressureElem = _elem->GetElement("air_pressure"); - return this->dataPtr->airPressure->PopulateElement(airPressureElem); + sdf::ElementPtr airPressureElem = elem->GetElement("air_pressure"); + airPressureElem->Copy(this->dataPtr->airPressure->ToElement()); } // altimeter else if (this->Type() == sdf::SensorType::ALTIMETER && this->dataPtr->altimeter) { - sdf::ElementPtr altimeterElem = _elem->GetElement("altimeter"); - return this->dataPtr->altimeter->PopulateElement(altimeterElem); + sdf::ElementPtr altimeterElem = elem->GetElement("altimeter"); + altimeterElem->Copy(this->dataPtr->altimeter->ToElement()); } // camera, depth, thermal, segmentation else if (this->CameraSensor()) { - sdf::ElementPtr cameraElem = _elem->GetElement("camera"); - return this->dataPtr->camera->PopulateElement(cameraElem); + sdf::ElementPtr cameraElem = elem->GetElement("camera"); + cameraElem->Copy(this->dataPtr->camera->ToElement()); } // force torque else if (this->Type() == sdf::SensorType::FORCE_TORQUE && this->dataPtr->forceTorque) { - sdf::ElementPtr forceTorqueElem = _elem->GetElement("force_torque"); - return this->dataPtr->forceTorque->PopulateElement(forceTorqueElem); + sdf::ElementPtr forceTorqueElem = elem->GetElement("force_torque"); + forceTorqueElem->Copy(this->dataPtr->forceTorque->ToElement()); } // imu else if (this->Type() == sdf::SensorType::IMU && this->dataPtr->imu) { - sdf::ElementPtr imuElem = _elem->GetElement("imu"); - return this->dataPtr->imu->PopulateElement(imuElem); + sdf::ElementPtr imuElem = elem->GetElement("imu"); + imuElem->Copy(this->dataPtr->imu->ToElement()); } // lidar, gpu_lidar else if ((this->Type() == sdf::SensorType::GPU_LIDAR || this->Type() == sdf::SensorType::LIDAR) && this->dataPtr->lidar) { - sdf::ElementPtr rayElem = (_elem->HasElement("ray")) ? - _elem->GetElement("ray") : _elem->GetElement("lidar"); - return this->dataPtr->lidar->PopulateElement(rayElem); + sdf::ElementPtr rayElem = (elem->HasElement("ray")) ? + elem->GetElement("ray") : elem->GetElement("lidar"); + rayElem->Copy(this->dataPtr->lidar->ToElement()); } // magnetometer else if (this->Type() == sdf::SensorType::MAGNETOMETER && this->dataPtr->magnetometer) { - sdf::ElementPtr magnetometerElem = _elem->GetElement("magnetometer"); - return this->dataPtr->magnetometer->PopulateElement(magnetometerElem); + sdf::ElementPtr magnetometerElem = elem->GetElement("magnetometer"); + magnetometerElem->Copy(this->dataPtr->magnetometer->ToElement()); } - - std::cout << "Conversion of sensor type: [" << this->TypeStr() << "] from " - << "SDF DOM to Element is not supported yet." << std::endl; - return false; + else + { + std::cout << "Conversion of sensor type: [" << this->TypeStr() << "] from " + << "SDF DOM to Element is not supported yet." << std::endl; + } + return elem; } diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 6881022c9..5873d44fa 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -502,3 +502,46 @@ TEST(DOMSensor, MutableSensors) sensor.NavSatSensor()->HorizontalPositionNoise().Mean(), 2.0); } } + +///////////////////////////////////////////////// +TEST(DOMSensor, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::Sensor sensor; + sensor.SetName("my_sensor"); + sensor.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor.SetType(sdf::SensorType::MAGNETOMETER); + sensor.SetPoseRelativeTo("a_frame"); + sensor.SetUpdateRate(0.123); + + sdf::Noise noise; + noise.SetMean(0.1); + sdf::Magnetometer mag; + mag.SetXNoise(noise); + sensor.SetMagnetometerSensor(mag); + + sdf::ElementPtr sensorElem = sensor.ToElement(); + EXPECT_NE(nullptr, sensorElem); + EXPECT_EQ(nullptr, sensor.Element()); + + // verify values after loading the element back + sdf::Sensor sensor2; + sensor2.Load(sensorElem); + + EXPECT_EQ("my_sensor", sensor2.Name()); + EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); + EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); + EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); + ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); + EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), + sensor2.MagnetometerSensor()->XNoise().Mean()); + EXPECT_DOUBLE_EQ(0.123, sensor2.UpdateRate()); + + // make changes to DOM and verify ToElement produces updated values + sensor2.SetUpdateRate(1.23); + sdf::ElementPtr sensor2Elem = sensor2.ToElement(); + EXPECT_NE(nullptr, sensor2Elem); + sdf::Sensor sensor3; + sensor3.Load(sensor2Elem); + EXPECT_DOUBLE_EQ(1.23, sensor3.UpdateRate()); +} diff --git a/test/integration/sdf_dom_conversion.cc b/test/integration/sdf_dom_conversion.cc index 4ac045898..dc5cbb8a8 100644 --- a/test/integration/sdf_dom_conversion.cc +++ b/test/integration/sdf_dom_conversion.cc @@ -62,11 +62,8 @@ TEST(SDFDomConversion, Sensors) // altimeter { const sdf::Sensor *sensor = link->SensorByIndex(0); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto altimeterSensor = std::make_unique(); altimeterSensor->Load(sensorElem); @@ -87,11 +84,8 @@ TEST(SDFDomConversion, Sensors) // camera { const sdf::Sensor *sensor = link->SensorByName("camera_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto cameraSensor = std::make_unique(); cameraSensor->Load(sensorElem); @@ -142,11 +136,8 @@ TEST(SDFDomConversion, Sensors) // depth { const sdf::Sensor *sensor = link->SensorByName("depth_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto depthSensor = std::make_unique(); depthSensor->Load(sensorElem); @@ -163,11 +154,8 @@ TEST(SDFDomConversion, Sensors) // rgbd { const sdf::Sensor *sensor = link->SensorByName("rgbd_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto rgbdSensor = std::make_unique(); rgbdSensor->Load(sensorElem); @@ -185,11 +173,8 @@ TEST(SDFDomConversion, Sensors) // thermal { const sdf::Sensor *sensor = link->SensorByName("thermal_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto thermalSensor = std::make_unique(); thermalSensor->Load(sensorElem); @@ -207,11 +192,8 @@ TEST(SDFDomConversion, Sensors) // segmentation { const sdf::Sensor *sensor = link->SensorByName("segmentation_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto segmentationSensor = std::make_unique(); segmentationSensor->Load(sensorElem); @@ -231,11 +213,8 @@ TEST(SDFDomConversion, Sensors) // force torque { const sdf::Sensor *sensor = link->SensorByName("force_torque_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto forceTorqueSensor = std::make_unique(); forceTorqueSensor->Load(sensorElem); @@ -250,11 +229,8 @@ TEST(SDFDomConversion, Sensors) // gpu lidar { const sdf::Sensor *sensor = link->SensorByName("gpu_lidar_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto gpuLidarSensor = std::make_unique(); gpuLidarSensor->Load(sensorElem); @@ -285,11 +261,8 @@ TEST(SDFDomConversion, Sensors) // imu { const sdf::Sensor *sensor = link->SensorByName("imu_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto imuSensor = std::make_unique(); imuSensor->Load(sensorElem); @@ -355,11 +328,8 @@ TEST(SDFDomConversion, Sensors) // magnetometer { const sdf::Sensor *sensor = link->SensorByName("magnetometer_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto magnetometerSensor = std::make_unique(); magnetometerSensor->Load(sensorElem); @@ -383,11 +353,8 @@ TEST(SDFDomConversion, Sensors) // air pressure { const sdf::Sensor *sensor = link->SensorByName("air_pressure_sensor"); - sdf::ElementPtr sensorElem(new sdf::Element); - sdf::initFile("sensor.sdf", sensorElem); - // convert to sdf element and load it back - sensor->PopulateElement(sensorElem); + sdf::ElementPtr sensorElem = sensor->ToElement(); auto airPressureSensor = std::make_unique(); airPressureSensor->Load(sensorElem); From e82c05716e3f5536848e42642e67e32ee22b9944 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 22 Nov 2021 16:33:44 -0800 Subject: [PATCH 41/60] Convert Joint DOM to Element (#759) * update api Signed-off-by: Ian Chen * add more tests Signed-off-by: Ian Chen * noise to elem test Signed-off-by: Ian Chen * remove unused var Signed-off-by: Ian Chen * convert joint dom to element Signed-off-by: Ian Chen * check expressed_in attr Signed-off-by: Ian Chen --- include/sdf/Joint.hh | 5 ++ include/sdf/JointAxis.hh | 6 ++ src/Joint.cc | 83 ++++++++++++++++++++++++++ src/JointAxis.cc | 41 +++++++++++++ src/Joint_TEST.cc | 47 +++++++++++++++ test/integration/sdf_dom_conversion.cc | 69 +++++++++++++++++++++ 6 files changed, 251 insertions(+) diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index fc1eabaa6..55f855c61 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -225,6 +225,11 @@ namespace sdf /// \sa bool SensorNameExists(const std::string &_name) const public: const Sensor *SensorByName(const std::string &_name) const; + /// \brief Create and return an SDF element filled with data from this + /// joint. + /// \return SDF element pointer with updated joint values. + public: sdf::ElementPtr ToElement() const; + /// \brief Give the scoped FrameAttachedToGraph to be used for resolving /// parent and child link names. This is private and is intended to be /// called by Model::Load. diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index d788d6f2d..b8d41fb97 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -221,6 +221,12 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Create and return an SDF element filled with data from this + /// joint axis. + /// \param[in] _index Index of this joint axis + /// \return SDF element pointer with updated joint values. + public: sdf::ElementPtr ToElement(unsigned int _index = 0u) const; + /// \brief Give the name of the xml parent of this object, to be used /// for resolving poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. diff --git a/src/Joint.cc b/src/Joint.cc index c7847bf5b..212141688 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -23,6 +23,7 @@ #include "sdf/Error.hh" #include "sdf/Joint.hh" #include "sdf/JointAxis.hh" +#include "sdf/parser.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" @@ -445,3 +446,85 @@ const Sensor *Joint::SensorByName(const std::string &_name) const } return nullptr; } + +///////////////////////////////////////////////// +sdf::ElementPtr Joint::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("joint.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + std::string jointType = "invalid"; + switch (this->Type()) + { + case JointType::BALL: + jointType = "ball"; + break; + case JointType::CONTINUOUS: + jointType = "continuous"; + break; + case JointType::FIXED: + jointType = "fixed"; + break; + case JointType::PRISMATIC: + jointType = "prismatic"; + break; + case JointType::GEARBOX: + jointType = "gearbox"; + break; + case JointType::REVOLUTE: + jointType = "revolute"; + break; + case JointType::REVOLUTE2: + jointType = "revolute2"; + break; + case JointType::SCREW: + jointType = "screw"; + break; + case JointType::UNIVERSAL: + jointType = "universal"; + break; + default: + break; + } + + elem->GetAttribute("type")->Set(jointType); + elem->GetElement("parent")->Set(this->ParentLinkName()); + elem->GetElement("child")->Set(this->ChildLinkName()); + for (unsigned int i = 0u; i < 2u; ++i) + { + const JointAxis *axis = this->Axis(i); + if (!axis) + break; + + std::string axisElemName = "axis"; + if (i > 0u) + axisElemName += std::to_string(i+1); + sdf::ElementPtr axisElem = elem->GetElement(axisElemName); + axisElem->Copy(axis->ToElement(i)); + } + + for (uint64_t i = 0u; i < this->SensorCount(); ++i) + { + const Sensor *sensor = this->SensorByIndex(i); + if (!sensor) + continue; + sdf::ElementPtr sensorElem = elem->GetElement("sensor"); + sensorElem->Copy(sensor->ToElement()); + } + + if (this->Type() == JointType::SCREW) + elem->GetElement("thread_pitch")->Set(this->ThreadPitch()); + + // gearbox_ratio, gearbox_reference_box, and physcs elements are not yet + // supported. + return elem; +} diff --git a/src/JointAxis.cc b/src/JointAxis.cc index af2fc1b73..5205fbcc2 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -23,6 +23,7 @@ #include "sdf/Assert.hh" #include "sdf/Error.hh" #include "sdf/JointAxis.hh" +#include "sdf/parser.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" #include "Utils.hh" @@ -373,3 +374,43 @@ sdf::ElementPtr JointAxis::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +sdf::ElementPtr JointAxis::ToElement(unsigned int _index) const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("joint.sdf", elem); + + std::string axisElemName = "axis"; + if (_index > 0u) + axisElemName += std::to_string(_index + 1); + sdf::ElementPtr axisElem = elem->GetElement(axisElemName); + sdf::ElementPtr xyzElem = axisElem->GetElement("xyz"); + xyzElem->Set(this->Xyz()); + if (!this->XyzExpressedIn().empty()) + { + 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()); + dynElem->GetElement("spring_reference")->Set( + this->SpringReference()); + dynElem->GetElement("spring_stiffness")->Set( + this->SpringStiffness()); + + sdf::ElementPtr limitElem = axisElem->GetElement("limit"); + limitElem->GetElement("lower")->Set(this->Lower()); + limitElem->GetElement("upper")->Set(this->Upper()); + limitElem->GetElement("effort")->Set(this->Effort()); + limitElem->GetElement("velocity")->Set(this->MaxVelocity()); + limitElem->GetElement("stiffness")->Set(this->Stiffness()); + limitElem->GetElement("dissipation")->Set(this->Dissipation()); + return axisElem; +} diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index 65d3cebca..e00463c20 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -250,3 +250,50 @@ TEST(DOMJoint, CopyAssignmentAfterMove) EXPECT_EQ(joint1Axis.Xyz(), joint2.Axis(0)->Xyz()); EXPECT_EQ(joint1Axis1.Xyz(), joint2.Axis(1)->Xyz()); } + +///////////////////////////////////////////////// +TEST(DOMJoint, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::Joint joint; + joint.SetRawPose({-1, -2, -3, 0, IGN_PI, 0}); + joint.SetPoseRelativeTo("link"); + joint.SetName("test_joint"); + joint.SetParentLinkName("parent"); + joint.SetChildLinkName("child"); + joint.SetType(sdf::JointType::BALL); + sdf::JointAxis axis; + EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty()); + joint.SetAxis(0, axis); + sdf::JointAxis axis1; + EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); + joint.SetAxis(1, axis1); + + sdf::ElementPtr jointElem = joint.ToElement(); + EXPECT_NE(nullptr, jointElem); + EXPECT_EQ(nullptr, joint.Element()); + + // verify values after loading the element back + sdf::Joint joint2; + joint2.Load(jointElem); + + EXPECT_EQ(ignition::math::Pose3d(-1, -2, -3, 0, IGN_PI, 0), + joint2.RawPose()); + EXPECT_EQ("link", joint2.PoseRelativeTo()); + EXPECT_EQ("test_joint", joint2.Name()); + EXPECT_EQ("parent", joint2.ParentLinkName()); + EXPECT_EQ("child", joint2.ChildLinkName()); + EXPECT_EQ(sdf::JointType::BALL, joint2.Type()); + ASSERT_TRUE(nullptr != joint2.Axis(0)); + ASSERT_TRUE(nullptr != joint2.Axis(1)); + EXPECT_EQ(axis.Xyz(), joint2.Axis(0)->Xyz()); + EXPECT_EQ(axis1.Xyz(), joint2.Axis(1)->Xyz()); + + // make changes to DOM and verify ToElement produces updated values + joint2.SetParentLinkName("new_parent"); + sdf::ElementPtr joint2Elem = joint2.ToElement(); + EXPECT_NE(nullptr, joint2Elem); + sdf::Joint joint3; + joint3.Load(joint2Elem); + EXPECT_EQ("new_parent", joint3.ParentLinkName()); +} diff --git a/test/integration/sdf_dom_conversion.cc b/test/integration/sdf_dom_conversion.cc index dc5cbb8a8..9fbd4c7dc 100644 --- a/test/integration/sdf_dom_conversion.cc +++ b/test/integration/sdf_dom_conversion.cc @@ -22,7 +22,9 @@ #include "sdf/Altimeter.hh" #include "sdf/Camera.hh" #include "sdf/Element.hh" +#include "sdf/ForceTorque.hh" #include "sdf/Imu.hh" +#include "sdf/Joint.hh" #include "sdf/Lidar.hh" #include "sdf/Light.hh" #include "sdf/Link.hh" @@ -531,3 +533,70 @@ TEST(SDFDomConversion, Lights) EXPECT_EQ(ignition::math::Vector3d(4.0, 5.0, 6.0), dirLight->Direction()); } } + +////////////////////////////////////////////////// +TEST(SDFDomConversion, Joints) +{ + // this test loads the joint_sensors.sdf test file, then + // 1) converts each type of sensor DOM to Element + // 2) loads the Element back to DOM, + // 3) verify the values + // Some of the verification code is adapted from joint_dom.cc + const std::string testFile = + sdf::testing::TestFile("sdf", "joint_sensors.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (auto e : errors) + std::cout << e << std::endl; + EXPECT_TRUE(errors.empty()); + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + + // Get joint + const sdf::Joint *j = model->JointByName("joint"); + ASSERT_NE(nullptr, j); + + // convert to sdf element and load it back + sdf::ElementPtr jointElem = j->ToElement(); + auto joint = std::make_unique(); + joint->Load(jointElem); + ASSERT_NE(nullptr, joint); + EXPECT_EQ("joint", joint->Name()); + EXPECT_EQ(1u, joint->SensorCount()); + + EXPECT_EQ("link1", joint->ParentLinkName()); + EXPECT_EQ("link2", joint->ChildLinkName()); + EXPECT_EQ(sdf::JointType::FIXED, joint->Type()); + + // Get the force_torque sensor + const sdf::Sensor *forceTorqueSensor = + joint->SensorByName("force_torque_sensor"); + ASSERT_NE(nullptr, forceTorqueSensor); + EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name()); + EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + forceTorqueSensor->RawPose()); + auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor(); + ASSERT_NE(nullptr, forceTorqueSensorObj); + EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorObj->Frame()); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, + forceTorqueSensorObj->MeasureDirection()); + + EXPECT_DOUBLE_EQ(0.0, forceTorqueSensorObj->ForceXNoise().Mean()); + EXPECT_DOUBLE_EQ(0.1, forceTorqueSensorObj->ForceXNoise().StdDev()); + EXPECT_DOUBLE_EQ(1.0, forceTorqueSensorObj->ForceYNoise().Mean()); + EXPECT_DOUBLE_EQ(1.1, forceTorqueSensorObj->ForceYNoise().StdDev()); + EXPECT_DOUBLE_EQ(2.0, forceTorqueSensorObj->ForceZNoise().Mean()); + EXPECT_DOUBLE_EQ(2.1, forceTorqueSensorObj->ForceZNoise().StdDev()); + + EXPECT_DOUBLE_EQ(3.0, forceTorqueSensorObj->TorqueXNoise().Mean()); + EXPECT_DOUBLE_EQ(3.1, forceTorqueSensorObj->TorqueXNoise().StdDev()); + EXPECT_DOUBLE_EQ(4.0, forceTorqueSensorObj->TorqueYNoise().Mean()); + EXPECT_DOUBLE_EQ(4.1, forceTorqueSensorObj->TorqueYNoise().StdDev()); + EXPECT_DOUBLE_EQ(5.0, forceTorqueSensorObj->TorqueZNoise().Mean()); + EXPECT_DOUBLE_EQ(5.1, forceTorqueSensorObj->TorqueZNoise().StdDev()); +} From d1b096cf66a361d2c5cdec06232dc2a0a2399c23 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 23 Nov 2021 15:46:09 -0800 Subject: [PATCH 42/60] Prepare for 12.2.0 (#760) * Prepare for 12.2.0 Signed-off-by: Nate Koenig * Updated changelog Signed-off-by: Nate Koenig * fix changelog Signed-off-by: Steve Peters Co-authored-by: Nate Koenig Co-authored-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 71 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 22dd03fe0..a813b647f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ project (sdformat12) set (SDF_PROTOCOL_VERSION 1.9) set (SDF_MAJOR_VERSION 12) -set (SDF_MINOR_VERSION 1) +set (SDF_MINOR_VERSION 2) set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) diff --git a/Changelog.md b/Changelog.md index 542e3282c..cbb98f660 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,76 @@ ## libsdformat 12.X +### libsdformat 12.2.0 (2021-11-23) + +1. Convert Joint DOM to Element. + * [Pull request #759](https://github.com/ignitionrobotics/sdformat/pull/759) + +1. Populate light sdf::ElementPtr from Light DOM + * [Pull request #755](https://github.com/ignitionrobotics/sdformat/pull/755) + +1. Add function to convert Sensor DOM to sdf::ElementPtr + * [Pull request #753](https://github.com/ignitionrobotics/sdformat/pull/753) + * [Pull request #757](https://github.com/ignitionrobotics/sdformat/pull/757) + +1. Support wide angle camera. + * [Pull request #744](https://github.com/ignitionrobotics/sdformat/pull/744) + +1. Forward ports + * [Pull request #756](https://github.com/ignitionrobotics/sdformat/pull/756) + * [Pull request #734](https://github.com/ignitionrobotics/sdformat/pull/734) + * [Pull request #738](https://github.com/ignitionrobotics/sdformat/pull/738) + * [Pull request #750](https://github.com/ignitionrobotics/sdformat/pull/750) + * [Pull request #752](https://github.com/ignitionrobotics/sdformat/pull/752) + +1. Changelog links to BitBucket backup. + * [Pull request #237](https://github.com/ignitionrobotics/sdformat/pull/237) + +1. Update BitBucket links. + * [Pull request #248](https://github.com/ignitionrobotics/sdformat/pull/248) + +1. Cherry-pick [sdf4] Update BitBucket links -> sdf6 + * [Pull request #258](https://github.com/ignitionrobotics/sdformat/pull/258) + +1. Patch popen/pclose method for Windows. + * [Pull request #297](https://github.com/ignitionrobotics/sdformat/pull/297) + +1. Parse rpyOffset as radians + * [Pull request #497](https://github.com/ignitionrobotics/sdformat/pull/497) + +1. Fix flattening logic for nested model names (sdf6) + * [Pull request #597](https://github.com/ignitionrobotics/sdformat/pull/597) + +1. Translate poses of nested models inside other nested models (sdf6). + * [Pull request #596](https://github.com/ignitionrobotics/sdformat/pull/596) + +1. Use Ubuntu bionic in CI + * [Pull request #626](https://github.com/ignitionrobotics/sdformat/pull/626) + +1. Create CODEOWNERS with azeey and scpeters. + * [Pull request #650](https://github.com/ignitionrobotics/sdformat/pull/650) + +1. Remove bitbucket-pipelines. + * [Pull request #674](https://github.com/ignitionrobotics/sdformat/pull/674) + +1. Check joint parent/child names in Root::Load. + * [Pull request #727](https://github.com/ignitionrobotics/sdformat/pull/727) + +1. Check joint parent link names in Model::Load. + * [Pull request #726](https://github.com/ignitionrobotics/sdformat/pull/726) + +1. Add Joint DOM API to access joint sensors + * [Pull request #517](https://github.com/ignitionrobotics/sdformat/pull/517) + +1. Remove outdated deprecation note from parser_urdf.hh + * [Pull request #740](https://github.com/ignitionrobotics/sdformat/pull/740) + +1. DOC: only allow one canonical_link attribute + * [Pull request #716](https://github.com/ignitionrobotics/sdformat/pull/716) + +1. Fix URDF fixed joint reduction of plugins + * [Pull request #500](https://github.com/ignitionrobotics/sdformat/pull/500) + * [Pull request #745](https://github.com/ignitionrobotics/sdformat/pull/745) + ### libsdformat 12.1.0 (2021-11-09) 1. Support accessing mutable sensor types. From b95f44d7cff03054297495c26332d80789c77b2d Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 2 Dec 2021 05:15:55 +0800 Subject: [PATCH 43/60] Fix empty pose parsing fail for rotation_format='quat_xyzw' (#729) The correct default value is now computed based on the attributes of the pose, so `` will have the the value '0 0 0 0 0 0 1' instead of the default string specified in pose.sdf. Signed-off-by: Aaron Chong Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- include/sdf/Param.hh | 19 +- src/Param.cc | 258 +++++++++++++++--- src/Param_TEST.cc | 38 +++ src/parser.cc | 2 + .../include_custom_model_expected_output.sdf | 2 +- test/integration/pose_1_9_sdf.cc | 73 +++-- test/sdf/pose_1_9.sdf | 10 + 7 files changed, 330 insertions(+), 72 deletions(-) diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index 2fc931a60..d46204be3 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -325,11 +325,6 @@ namespace sdf return _out; } - /// \brief Private method to set the Element from a passed-in string. - /// \param[in] _value Value to set the parameter to. - /// \return True if the parameter was successfully set, false otherwise. - private: bool ValueFromString(const std::string &_value); - /// \brief Private data private: std::unique_ptr dataPtr; }; @@ -382,7 +377,7 @@ namespace sdf public: bool ignoreParentAttributes; /// \brief This parameter's value that was provided as a string - public: std::string strValue; + public: std::optional strValue; /// \brief This parameter's default value that was provided as a string public: std::string defaultStrValue; @@ -406,6 +401,18 @@ namespace sdf const std::string &_valueStr, ParamVariant &_valueToSet) const; + /// \brief Method used to get the string representation from a ParamVariant + /// \param[in] _config Print configuration for the string output + /// \param[in] _typeName The data type of the value + /// \param[in] _value The value + /// \param[in] _valueStr The string representation of the value + /// \return True if the string was successfully retrieved from the value, + /// false otherwise. + public: bool StringFromValueImpl(const PrintConfig &_config, + const std::string &_typeName, + const ParamVariant &_value, + std::string &_valueStr) const; + /// \brief Data type to string mapping /// \return The type as a string, empty string if unknown type public: template diff --git a/src/Param.cc b/src/Param.cc index ad6a30833..4c6ee2db1 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -73,8 +73,14 @@ Param::Param(const std::string &_key, const std::string &_typeName, this->dataPtr->ignoreParentAttributes = false; this->dataPtr->defaultStrValue = _default; - SDF_ASSERT(this->ValueFromString(_default), "Invalid parameter"); - this->dataPtr->defaultValue = this->dataPtr->value; + SDF_ASSERT( + this->dataPtr->ValueFromStringImpl( + this->dataPtr->typeName, + _default, + this->dataPtr->defaultValue), + "Invalid parameter"); + this->dataPtr->value = this->dataPtr->defaultValue; + this->dataPtr->strValue = std::nullopt; } ////////////////////////////////////////////////// @@ -302,18 +308,33 @@ void Param::Update() ////////////////////////////////////////////////// std::string Param::GetAsString(const PrintConfig &_config) const { - (void)_config; - StringStreamClassicLocale ss; - ss << this->dataPtr->strValue; - return ss.str(); + if (this->dataPtr->strValue.has_value() && !this->dataPtr->strValue->empty()) + { + return this->dataPtr->strValue.value(); + } + else if(!this->dataPtr->strValue.has_value()) + { + return this->dataPtr->defaultStrValue; + } + + return this->GetDefaultAsString(_config); } ////////////////////////////////////////////////// std::string Param::GetDefaultAsString(const PrintConfig &_config) const { - (void)_config; - StringStreamClassicLocale ss; + std::string defaultStr; + if (this->dataPtr->StringFromValueImpl(_config, + this->dataPtr->typeName, + this->dataPtr->defaultValue, + defaultStr)) + { + return defaultStr; + } + sdferr << "Unable to get string from default value, " + << "using ParamStreamer instead.\n"; + StringStreamClassicLocale ss; ss << ParamStreamer{ this->dataPtr->defaultValue }; return ss.str(); } @@ -324,11 +345,17 @@ std::optional Param::GetMinValueAsString( { if (this->dataPtr->minValue.has_value()) { - (void)_config; - StringStreamClassicLocale ss; + std::string valueStr; + if (!this->dataPtr->StringFromValueImpl(_config, + this->dataPtr->typeName, + this->dataPtr->minValue.value(), + valueStr)) + { + sdferr << "Unable to get min value as string.\n"; + return std::nullopt; + } - ss << ParamStreamer{ *this->dataPtr->minValue }; - return ss.str(); + return valueStr; } return std::nullopt; } @@ -339,11 +366,17 @@ std::optional Param::GetMaxValueAsString( { if (this->dataPtr->maxValue.has_value()) { - (void)_config; - StringStreamClassicLocale ss; + std::string valueStr; + if (!this->dataPtr->StringFromValueImpl(_config, + this->dataPtr->typeName, + this->dataPtr->maxValue.value(), + valueStr)) + { + sdferr << "Unable to get max value as string.\n"; + return std::nullopt; + } - ss << ParamStreamer{ *this->dataPtr->maxValue }; - return ss.str(); + return valueStr; } return std::nullopt; } @@ -461,8 +494,6 @@ bool ParsePoseUsingStringStream(const std::string &_input, const std::size_t defaultDesiredSize = 6u; std::size_t desiredSize = defaultDesiredSize; - std::string defaultValueStr = "0 0 0 0 0 0"; - for (const auto &p : _attributes) { const std::string key = p->GetKey(); @@ -487,7 +518,6 @@ bool ParsePoseUsingStringStream(const std::string &_input, else if (rotationFormat == "quat_xyzw") { desiredSize = 7u; - defaultValueStr = "0 0 0 0 0 0 1"; } else { @@ -506,8 +536,13 @@ bool ParsePoseUsingStringStream(const std::string &_input, return false; } - std::string input = _input.empty() ? defaultValueStr : _input; - StringStreamClassicLocale ss(input); + if (_input.empty()) + { + _value = ignition::math::Pose3d::Zero; + return true; + } + + StringStreamClassicLocale ss(_input); std::string token; std::array values; std::size_t valueIndex = 0; @@ -522,7 +557,7 @@ bool ParsePoseUsingStringStream(const std::string &_input, // Catch invalid argument exception from std::stod catch(std::invalid_argument &) { - sdferr << "Invalid argument. Unable to set value ["<< input + sdferr << "Invalid argument. Unable to set value ["<< _input << "] for key [" << _key << "].\n"; isValidPose = false; break; @@ -547,7 +582,7 @@ bool ParsePoseUsingStringStream(const std::string &_input, { sdferr << "The value for //pose[@rotation_format='" << rotationFormat << "'] must have " << desiredSize - << " values, but more than that were found in '" << input << "'.\n"; + << " values, but more than that were found in '" << _input << "'.\n"; isValidPose = false; break; } @@ -562,7 +597,7 @@ bool ParsePoseUsingStringStream(const std::string &_input, { sdferr << "The value for //pose[@rotation_format='" << rotationFormat << "'] must have " << desiredSize << " values, but " << valueIndex - << " were found instead in '" << input << "'.\n"; + << " were found instead in '" << _input << "'.\n"; return false; } @@ -588,17 +623,6 @@ bool ParsePoseUsingStringStream(const std::string &_input, return true; } -////////////////////////////////////////////////// -bool Param::ValueFromString(const std::string &_value) -{ - if (!this->dataPtr->ValueFromStringImpl( - this->dataPtr->typeName, _value, this->dataPtr->value)) - return false; - - this->dataPtr->strValue = _value; - return true; -} - ////////////////////////////////////////////////// bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, const std::string &_valueStr, @@ -761,6 +785,134 @@ bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, return true; } +///////////////////////////////////////////////// +bool PoseStringFromValue(const PrintConfig &_config, + const Param_V &_parentAttributes, + const ParamPrivate::ParamVariant &_value, + std::string &_valueStr) +{ + (void)_config; + StringStreamClassicLocale ss; + + const ignition::math::Pose3d *pose = + std::get_if(&_value); + if (!pose) + { + sdferr << "Unable to get pose value from variant.\n"; + return false; + } + + auto sanitizeZero = [](double _number) + { + StringStreamClassicLocale stream; + if (std::fpclassify(_number) == FP_ZERO) + { + stream << 0; + } + else + { + stream << _number; + } + return stream.str(); + }; + + const bool defaultInDegrees = false; + bool inDegrees = defaultInDegrees; + + const std::string defaultRotationFormat = "euler_rpy"; + std::string rotationFormat = defaultRotationFormat; + + // When @degrees and @rotation_format attributes are not set, a single space + // delimiter is used to prevent breaking behavior and tests. + const std::string defaultPosRotDelimiter = " "; + const std::string threeSpacedDelimiter = " "; + std::string posRotDelimiter = defaultPosRotDelimiter; + + for (const auto &p : _parentAttributes) + { + const std::string key = p->GetKey(); + + if (key == "degrees") + { + if (!p->Get(inDegrees)) + { + sdferr << "Unable to get //pose[@degrees] attribute as bool.\n"; + return false; + } + if (p->GetSet()) + { + posRotDelimiter = threeSpacedDelimiter; + } + } + else if (key == "rotation_format") + { + rotationFormat = p->GetAsString(); + if (p->GetSet()) + { + posRotDelimiter = threeSpacedDelimiter; + } + } + } + + if (rotationFormat == "quat_xyzw" && inDegrees) + { + sdferr << "Invalid pose with //pose[@degrees='true'] and " + << "//pose[@rotation_format='quat_xyzw'].\n"; + return false; + } + else if (rotationFormat == "quat_xyzw") + { + ss << pose->Pos() << posRotDelimiter + << sanitizeZero(pose->Rot().X()) << " " + << sanitizeZero(pose->Rot().Y()) << " " + << sanitizeZero(pose->Rot().Z()) << " " + << sanitizeZero(pose->Rot().W()); + _valueStr = ss.str(); + return true; + } + else if (rotationFormat == "euler_rpy" && inDegrees) + { + ss << pose->Pos() << posRotDelimiter + << sanitizeZero(IGN_RTOD(pose->Rot().Roll())) << " " + << sanitizeZero(IGN_RTOD(pose->Rot().Pitch())) << " " + << sanitizeZero(IGN_RTOD(pose->Rot().Yaw())); + _valueStr = ss.str(); + return true; + } + + ss << pose->Pos() << posRotDelimiter + << sanitizeZero(pose->Rot().Roll()) << " " + << sanitizeZero(pose->Rot().Pitch()) << " " + << sanitizeZero(pose->Rot().Yaw()); + _valueStr = ss.str(); + return true; +} + +///////////////////////////////////////////////// +bool ParamPrivate::StringFromValueImpl(const PrintConfig &_config, + const std::string &_typeName, + const ParamVariant &_value, + std::string &_valueStr) const +{ + if (_typeName == "ignition::math::Pose3d" || + _typeName == "pose" || + _typeName == "Pose") + { + const ElementPtr p = this->parentElement.lock(); + if (!this->ignoreParentAttributes && p) + { + return PoseStringFromValue( + _config, p->GetAttributes(), _value, _valueStr); + } + return PoseStringFromValue(_config, {}, _value, _valueStr); + } + + StringStreamClassicLocale ss; + ss << ParamStreamer{ _value }; + _valueStr = ss.str(); + return true; +} + ////////////////////////////////////////////////// bool Param::SetFromString(const std::string &_value, bool _ignoreParentAttributes) @@ -777,14 +929,18 @@ bool Param::SetFromString(const std::string &_value, else if (str.empty()) { this->dataPtr->value = this->dataPtr->defaultValue; + this->dataPtr->strValue = str; return true; } auto oldValue = this->dataPtr->value; - if (!this->ValueFromString(str)) + if (!this->dataPtr->ValueFromStringImpl(this->dataPtr->typeName, + str, + this->dataPtr->value)) { return false; } + this->dataPtr->strValue = str; // Check if the value is permitted if (!this->ValidateValue()) @@ -828,19 +984,35 @@ bool Param::SetParentElement(ElementPtr _parentElement) void Param::Reset() { this->dataPtr->value = this->dataPtr->defaultValue; - this->dataPtr->strValue = this->dataPtr->defaultStrValue; + this->dataPtr->strValue = std::nullopt; this->dataPtr->set = false; } ////////////////////////////////////////////////// bool Param::Reparse() { + std::string strToReparse; + if (this->dataPtr->strValue.has_value()) + { + strToReparse = this->dataPtr->strValue.value(); + } + // A default PrintConfig can be used here, as Reparse() is not called in the + // code path from the 'ign sdf -p' command. + else if (!this->dataPtr->StringFromValueImpl(PrintConfig(), + this->dataPtr->typeName, + this->dataPtr->defaultValue, + strToReparse)) + { + sdferr << "Failed to obtain string from default value during reparsing.\n"; + return false; + } + if (!this->dataPtr->ValueFromStringImpl( - this->dataPtr->typeName, this->dataPtr->strValue, this->dataPtr->value)) + this->dataPtr->typeName, strToReparse, this->dataPtr->value)) { if (const auto parentElement = this->dataPtr->parentElement.lock()) { - sdferr << "Failed to set value '" << this->dataPtr->strValue + sdferr << "Failed to set value '" << strToReparse << "' to key [" << this->GetKey() << "] for new parent element of name '" << parentElement->GetName() << "', reverting to previous value '" @@ -848,12 +1020,18 @@ bool Param::Reparse() } else { - sdferr << "Failed to set value '" << this->dataPtr->strValue + sdferr << "Failed to set value '" << strToReparse << "' to key [" << this->GetKey() << "] without a parent element, " << "reverting to previous value '" << this->GetAsString() << "'.\n"; } return false; } + // ValueFromStringImpl might make assumptions as to what the default value + // should be, so if strToReparse is empty, assign the correct default value. + if (strToReparse.empty()) + { + this->dataPtr->value = this->dataPtr->defaultValue; + } return true; } diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc index 17696b279..b5bdf975a 100644 --- a/src/Param_TEST.cc +++ b/src/Param_TEST.cc @@ -28,6 +28,7 @@ #include "sdf/Exception.hh" #include "sdf/Element.hh" #include "sdf/Param.hh" +#include "sdf/parser.hh" bool check_double(const std::string &num) { @@ -863,6 +864,43 @@ TEST(Param, ChangeParentElementFail) EXPECT_EQ(parent, poseElem); } +///////////////////////////////////////////////// +TEST(Param, PoseWithDefaultValue) +{ + using Pose = ignition::math::Pose3d; + + auto poseElem = std::make_shared(); + poseElem->SetName("pose"); + poseElem->AddValue("pose", "1 0 0 0 0 0", false); + poseElem->AddAttribute("rotation_format", "string", "euler_rpy", false); + + // Clone poseElem for testing Parm::SetFromString + auto poseElemClone = poseElem->Clone(); + + const std::string testString = R"( + + + )"; + + sdf::Errors errors; + EXPECT_TRUE(sdf::readString(testString, poseElem, errors)); + EXPECT_TRUE(errors.empty()) << errors; + + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), poseElem->Get()); + const std::string expectedString = + "1 0 0 0 0 0 1\n"; + EXPECT_STREQ(expectedString.c_str(), poseElem->ToString("").c_str()); + + // same test but using Param::SetFromString + poseElemClone->GetAttribute("rotation_format")->SetFromString("quat_xyzw"); + auto value = poseElemClone->GetValue(); + value->SetFromString(""); + value->Reparse(); + + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), poseElemClone->Get()); + EXPECT_STREQ(expectedString.c_str(), poseElemClone->ToString("").c_str()); +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/src/parser.cc b/src/parser.cc index b827678f6..a3cec154a 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1522,6 +1522,8 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, { if (!_sdf->GetValue()->Reparse()) return false; + if (!_sdf->GetValue()->SetFromString("")) + return false; } if (_sdf->GetCopyChildren()) diff --git a/test/integration/include_custom_model_expected_output.sdf b/test/integration/include_custom_model_expected_output.sdf index b078ce99b..e4584cd98 100644 --- a/test/integration/include_custom_model_expected_output.sdf +++ b/test/integration/include_custom_model_expected_output.sdf @@ -332,7 +332,7 @@ - 0 0 0 0 -0 0 + 0 0 0 0 0 0 diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc index 59c8a9974..c70a98037 100644 --- a/test/integration/pose_1_9_sdf.cc +++ b/test/integration/pose_1_9_sdf.cc @@ -156,6 +156,54 @@ TEST(Pose1_9, PoseExpressionFormats) EXPECT_EQ(Pose(1, 2, 3, 0.7071068, 0.7071068, 0, 0), link->Inertial().Pose()); } + + model = world->ModelByIndex(18); + ASSERT_NE(nullptr, model); + EXPECT_EQ("model_empty_quat_xyzw", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + model = world->ModelByIndex(19); + ASSERT_NE(nullptr, model); + EXPECT_EQ("model_empty_quat_xyzw_degrees_false", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); +} + +////////////////////////////////////////////////// +TEST(Pose1_9, PoseStringOutput) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << "" + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + sdf::Errors errors; + ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed, errors)); + ASSERT_TRUE(errors.empty()) << errors; + + sdf::Root root; + errors = root.Load(sdfParsed); + ASSERT_TRUE(errors.empty()) << errors; + + auto model = root.Model(); + ASSERT_NE(nullptr, model); + + auto elem = model->Element(); + ASSERT_NE(nullptr, elem); + + auto poseElem = elem->GetElement("pose"); + ASSERT_NE(nullptr, poseElem); + + auto poseParam = poseElem->GetValue(); + ASSERT_NE(nullptr, poseParam); + + const std::string strOutput = poseParam->GetAsString(); + EXPECT_EQ("0 0 0 0 0 0 1", strOutput); } ////////////////////////////////////////////////// @@ -272,31 +320,6 @@ TEST(Pose1_9, BadModelPoses) EXPECT_PRED2(contains, buffer.str(), "//pose[@degrees='true'] does not apply when parsing quaternions"); } - - { - buffer.str(""); - std::ostringstream stream; - stream - << "" - << " " - << " " - << " " - << " " - << ""; - - sdf::SDFPtr sdfParsed(new sdf::SDF()); - sdf::init(sdfParsed); - sdf::Errors errors; - EXPECT_FALSE(sdf::readString(stream.str(), sdfParsed, errors)); - EXPECT_FALSE(errors.empty()); - - // Setting //pose[@rotation_format='quat_xyzw'] should fail here as - // the defined default value from sdf/1.9/pose.sdf is '0 0 0 0 0 0' - // which is only a 6-tuple. - EXPECT_PRED2(contains, buffer.str(), - "//pose[@rotation_format='quat_xyzw'] must have 7 values, " - "but 6 were found instead in '0 0 0 0 0 0'"); - } } ////////////////////////////////////////////////// diff --git a/test/sdf/pose_1_9.sdf b/test/sdf/pose_1_9.sdf index 0cb17805a..aa60ab0ce 100644 --- a/test/sdf/pose_1_9.sdf +++ b/test/sdf/pose_1_9.sdf @@ -117,5 +117,15 @@ + + + + + + + + + + From 8707d41fa78e0fae975608e0b8fff530918fe5d4 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 1 Dec 2021 15:24:52 -0800 Subject: [PATCH 44/60] Added Add & Clear function to World, Model, and Link (#765) * Added Add & Clear function to World, Model, and Link Signed-off-by: Nate Koenig * spelling Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/Link.hh | 36 +++++++++++++++++ include/sdf/Model.hh | 27 +++++++++++++ include/sdf/World.hh | 27 +++++++++++++ sdf/1.9/actor.sdf | 4 +- src/Link.cc | 60 +++++++++++++++++++++++++++++ src/Link_TEST.cc | 92 ++++++++++++++++++++++++++++++++++++++++++++ src/Model.cc | 45 ++++++++++++++++++++++ src/Model_TEST.cc | 69 +++++++++++++++++++++++++++++++++ src/World.cc | 47 ++++++++++++++++++++++ src/World_TEST.cc | 72 ++++++++++++++++++++++++++++++++++ 10 files changed, 477 insertions(+), 2 deletions(-) diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 97751166a..eca492f77 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -252,6 +252,42 @@ namespace sdf /// \sa Model::SetEnableWind(bool) public: void SetEnableWind(bool _enableWind); + /// \brief Add a collision to the link. + /// \param[in] _collision Collision to add. + /// \return True if successful, false if a collision with the name already + /// exists. + public: bool AddCollision(const Collision &_collision); + + /// \brief Add a visual to the link. + /// \param[in] _visual Visual to add. + /// \return True if successful, false if a visual with the name already + /// exists. + public: bool AddVisual(const Visual &_visual); + + /// \brief Add a light to the link. + /// \param[in] _light Light to add. + /// \return True if successful, false if a light with the name already + /// exists. + public: bool AddLight(const Light &_light); + + /// \brief Add a sensor to the link. + /// \param[in] _sensor Sensor to add. + /// \return True if successful, false if a sensor with the name already + /// exists. + public: bool AddSensor(const Sensor &_sensor); + + /// \brief Remove all collisions + public: void ClearCollisions(); + + /// \brief Remove all visuals + public: void ClearVisuals(); + + /// \brief Remove all lights + public: void ClearLights(); + + /// \brief Remove all sensors + public: void ClearSensors(); + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 8b1f7771c..ac047de1d 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -346,6 +346,33 @@ namespace sdf public: const NestedInclude *InterfaceModelNestedIncludeByIndex( const uint64_t _index) const; + /// \brief Add a link to the model. + /// \param[in] _link Link to add. + /// \return True if successful, false if a link with the name already + /// exists. + public: bool AddLink(const Link &_link); + + /// \brief Add a joint to the model. + /// \param[in] _link 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 model to the model. + /// \param[in] _model Model to add. + /// \return True if successful, false if a model with the name already + /// exists. + public: bool AddModel(const Model &_model); + + /// \brief Remove all links. + public: void ClearLinks(); + + /// \brief Remove all joints. + public: void ClearJoints(); + + /// \brief Remove all models. + public: void ClearModels(); + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Root::Load or /// World::SetPoseRelativeToGraph if this is a standalone model and diff --git a/include/sdf/World.hh b/include/sdf/World.hh index b0d4d6515..4280cc57b 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -180,6 +180,33 @@ namespace sdf /// \return True if there exists a model with the given name. public: bool ModelNameExists(const std::string &_name) const; + /// \brief Add a model to the world. + /// \param[in] _model Model to add. + /// \return True if successful, false if a model with the name already + /// exists. + public: bool AddModel(const Model &_model); + + /// \brief Add an actor to the world. + /// \param[in] _actor Actor to add. + /// \return True if successful, false if an actor with the name already + /// exists. + public: bool AddActor(const Actor &_actor); + + /// \brief Add a light to the world. + /// \param[in] _light Light to add. + /// \return True if successful, false if a lights with the name already + /// exists. + public: bool AddLight(const Light &_light); + + /// \brief Remove all models. + public: void ClearModels(); + + /// \brief Remove all models. + public: void ClearActors(); + + /// \brief Remove all models. + public: void ClearLights(); + /// \brief Get the number of actors. /// \return Number of actors contained in this World object. public: uint64_t ActorCount() const; diff --git a/sdf/1.9/actor.sdf b/sdf/1.9/actor.sdf index 205a2c4c5..4ff1c4cc6 100644 --- a/sdf/1.9/actor.sdf +++ b/sdf/1.9/actor.sdf @@ -74,9 +74,9 @@ The pose which should be reached at the given time. - + - + diff --git a/src/Link.cc b/src/Link.cc index 67160226a..7a5ed5846 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -497,3 +497,63 @@ void Link::SetEnableWind(const bool _enableWind) { this->dataPtr->enableWind = _enableWind; } + +////////////////////////////////////////////////// +bool Link::AddCollision(const Collision &_collision) +{ + if (this->CollisionNameExists(_collision.Name())) + return false; + this->dataPtr->collisions.push_back(_collision); + return true; +} + +////////////////////////////////////////////////// +bool Link::AddVisual(const Visual &_visual) +{ + if (this->VisualNameExists(_visual.Name())) + return false; + this->dataPtr->visuals.push_back(_visual); + return true; +} + +////////////////////////////////////////////////// +bool Link::AddLight(const Light &_light) +{ + if (this->LightNameExists(_light.Name())) + return false; + this->dataPtr->lights.push_back(_light); + return true; +} + +////////////////////////////////////////////////// +bool Link::AddSensor(const Sensor &_sensor) +{ + if (this->SensorNameExists(_sensor.Name())) + return false; + this->dataPtr->sensors.push_back(_sensor); + return true; +} + +////////////////////////////////////////////////// +void Link::ClearCollisions() +{ + this->dataPtr->collisions.clear(); +} + +////////////////////////////////////////////////// +void Link::ClearVisuals() +{ + this->dataPtr->visuals.clear(); +} + +////////////////////////////////////////////////// +void Link::ClearLights() +{ + this->dataPtr->lights.clear(); +} + +////////////////////////////////////////////////// +void Link::ClearSensors() +{ + this->dataPtr->sensors.clear(); +} diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 8b11381ff..6dd29d8c5 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -211,3 +211,95 @@ TEST(DOMLink, InvalidInertia) EXPECT_DOUBLE_EQ(3.4, link.Inertial().MassMatrix().OffDiagonalMoments().Z()); EXPECT_FALSE(link.Inertial().MassMatrix().IsValid()); } + +///////////////////////////////////////////////// +TEST(DOMLink, AddCollision) +{ + sdf::Link link; + EXPECT_EQ(0u, link.CollisionCount()); + + sdf::Collision collision; + collision.SetName("collision1"); + EXPECT_TRUE(link.AddCollision(collision)); + EXPECT_EQ(1u, link.CollisionCount()); + EXPECT_FALSE(link.AddCollision(collision)); + EXPECT_EQ(1u, link.CollisionCount()); + + link.ClearCollisions(); + EXPECT_EQ(0u, link.CollisionCount()); + + EXPECT_TRUE(link.AddCollision(collision)); + EXPECT_EQ(1u, link.CollisionCount()); + const sdf::Collision *collisionFromLink = link.CollisionByIndex(0); + ASSERT_NE(nullptr, collisionFromLink); + EXPECT_EQ(collisionFromLink->Name(), collision.Name()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, AddVisual) +{ + sdf::Link link; + EXPECT_EQ(0u, link.VisualCount()); + + sdf::Visual visual; + visual.SetName("visual1"); + EXPECT_TRUE(link.AddVisual(visual)); + EXPECT_EQ(1u, link.VisualCount()); + EXPECT_FALSE(link.AddVisual(visual)); + EXPECT_EQ(1u, link.VisualCount()); + + link.ClearVisuals(); + EXPECT_EQ(0u, link.VisualCount()); + + EXPECT_TRUE(link.AddVisual(visual)); + EXPECT_EQ(1u, link.VisualCount()); + const sdf::Visual *visualFromLink = link.VisualByIndex(0); + ASSERT_NE(nullptr, visualFromLink); + EXPECT_EQ(visualFromLink->Name(), visual.Name()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, AddLight) +{ + sdf::Link link; + EXPECT_EQ(0u, link.LightCount()); + + sdf::Light light; + light.SetName("light1"); + EXPECT_TRUE(link.AddLight(light)); + EXPECT_EQ(1u, link.LightCount()); + EXPECT_FALSE(link.AddLight(light)); + EXPECT_EQ(1u, link.LightCount()); + + link.ClearLights(); + EXPECT_EQ(0u, link.LightCount()); + + EXPECT_TRUE(link.AddLight(light)); + EXPECT_EQ(1u, link.LightCount()); + const sdf::Light *lightFromLink = link.LightByIndex(0); + ASSERT_NE(nullptr, lightFromLink); + EXPECT_EQ(lightFromLink->Name(), light.Name()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, AddSensor) +{ + sdf::Link link; + EXPECT_EQ(0u, link.SensorCount()); + + sdf::Sensor sensor; + sensor.SetName("sensor1"); + EXPECT_TRUE(link.AddSensor(sensor)); + EXPECT_EQ(1u, link.SensorCount()); + EXPECT_FALSE(link.AddSensor(sensor)); + EXPECT_EQ(1u, link.SensorCount()); + + link.ClearSensors(); + EXPECT_EQ(0u, link.SensorCount()); + + EXPECT_TRUE(link.AddSensor(sensor)); + EXPECT_EQ(1u, link.SensorCount()); + const sdf::Sensor *sensorFromLink = link.SensorByIndex(0); + ASSERT_NE(nullptr, sensorFromLink); + EXPECT_EQ(sensorFromLink->Name(), sensor.Name()); +} diff --git a/src/Model.cc b/src/Model.cc index 791510f69..361b23243 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -790,3 +790,48 @@ const NestedInclude *Model::InterfaceModelNestedIncludeByIndex( return &this->dataPtr->interfaceModels[_index].first; return nullptr; } + +////////////////////////////////////////////////// +bool Model::AddLink(const Link &_link) +{ + if (this->LinkNameExists(_link.Name())) + return false; + this->dataPtr->links.push_back(_link); + return true; +} + +////////////////////////////////////////////////// +bool Model::AddJoint(const Joint &_joint) +{ + if (this->JointNameExists(_joint.Name())) + return false; + this->dataPtr->joints.push_back(_joint); + return true; +} + +////////////////////////////////////////////////// +bool Model::AddModel(const Model &_model) +{ + if (this->ModelNameExists(_model.Name())) + return false; + this->dataPtr->models.push_back(_model); + return true; +} + +////////////////////////////////////////////////// +void Model::ClearLinks() +{ + this->dataPtr->links.clear(); +} + +////////////////////////////////////////////////// +void Model::ClearJoints() +{ + this->dataPtr->joints.clear(); +} + +////////////////////////////////////////////////// +void Model::ClearModels() +{ + this->dataPtr->models.clear(); +} diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 06cc2e881..6054d1fe7 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -213,3 +213,72 @@ TEST(DOMModel, CopyAssignmentAfterMove) EXPECT_EQ("model2", model1.Name()); EXPECT_EQ("model1", model2.Name()); } + +///////////////////////////////////////////////// +TEST(DOMModel, AddLink) +{ + sdf::Model model; + EXPECT_EQ(0u, model.LinkCount()); + + sdf::Link link; + link.SetName("link1"); + EXPECT_TRUE(model.AddLink(link)); + EXPECT_EQ(1u, model.LinkCount()); + EXPECT_FALSE(model.AddLink(link)); + EXPECT_EQ(1u, model.LinkCount()); + + model.ClearLinks(); + EXPECT_EQ(0u, model.LinkCount()); + + EXPECT_TRUE(model.AddLink(link)); + EXPECT_EQ(1u, model.LinkCount()); + const sdf::Link *linkFromModel = model.LinkByIndex(0); + ASSERT_NE(nullptr, linkFromModel); + EXPECT_EQ(linkFromModel->Name(), link.Name()); +} + +///////////////////////////////////////////////// +TEST(DOMModel, AddJoint) +{ + sdf::Model model; + EXPECT_EQ(0u, model.JointCount()); + + sdf::Joint joint; + joint.SetName("joint1"); + EXPECT_TRUE(model.AddJoint(joint)); + EXPECT_EQ(1u, model.JointCount()); + EXPECT_FALSE(model.AddJoint(joint)); + EXPECT_EQ(1u, model.JointCount()); + + model.ClearJoints(); + EXPECT_EQ(0u, model.JointCount()); + + EXPECT_TRUE(model.AddJoint(joint)); + EXPECT_EQ(1u, model.JointCount()); + const sdf::Joint *jointFromModel = model.JointByIndex(0); + ASSERT_NE(nullptr, jointFromModel); + EXPECT_EQ(jointFromModel->Name(), joint.Name()); +} + +///////////////////////////////////////////////// +TEST(DOMModel, AddModel) +{ + sdf::Model model; + EXPECT_EQ(0u, model.ModelCount()); + + sdf::Model nestedModel; + nestedModel.SetName("model1"); + EXPECT_TRUE(model.AddModel(nestedModel)); + EXPECT_EQ(1u, model.ModelCount()); + EXPECT_FALSE(model.AddModel(nestedModel)); + EXPECT_EQ(1u, model.ModelCount()); + + model.ClearModels(); + EXPECT_EQ(0u, model.ModelCount()); + + EXPECT_TRUE(model.AddModel(nestedModel)); + EXPECT_EQ(1u, model.ModelCount()); + const sdf::Model *modelFromModel = model.ModelByIndex(0); + ASSERT_NE(nullptr, modelFromModel); + EXPECT_EQ(modelFromModel->Name(), nestedModel.Name()); +} diff --git a/src/World.cc b/src/World.cc index 42989ee49..64ed5ddb4 100644 --- a/src/World.cc +++ b/src/World.cc @@ -797,3 +797,50 @@ Errors World::Implementation::LoadSphericalCoordinates( return errors; } + +///////////////////////////////////////////////// +void World::ClearModels() +{ + this->dataPtr->models.clear(); +} + +///////////////////////////////////////////////// +void World::ClearActors() +{ + this->dataPtr->actors.clear(); +} + +///////////////////////////////////////////////// +void World::ClearLights() +{ + this->dataPtr->lights.clear(); +} + +///////////////////////////////////////////////// +bool World::AddModel(const Model &_model) +{ + if (this->ModelNameExists(_model.Name())) + return false; + this->dataPtr->models.push_back(_model); + return true; +} + +///////////////////////////////////////////////// +bool World::AddActor(const Actor &_actor) +{ + if (this->ActorNameExists(_actor.Name())) + return false; + this->dataPtr->actors.push_back(_actor); + + return true; +} + +///////////////////////////////////////////////// +bool World::AddLight(const Light &_light) +{ + if (this->LightNameExists(_light.Name())) + return false; + this->dataPtr->lights.push_back(_light); + + return true; +} diff --git a/src/World_TEST.cc b/src/World_TEST.cc index c2d67012c..7a76f33c8 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -18,6 +18,9 @@ #include #include #include +#include "sdf/Light.hh" +#include "sdf/Actor.hh" +#include "sdf/Model.hh" #include "sdf/World.hh" ///////////////////////////////////////////////// @@ -348,3 +351,72 @@ TEST(DOMWorld, SetScene) EXPECT_TRUE(world.Scene()->Shadows()); EXPECT_TRUE(world.Scene()->OriginVisual()); } + +///////////////////////////////////////////////// +TEST(DOMWorld, AddModel) +{ + sdf::World world; + EXPECT_EQ(0u, world.ModelCount()); + + sdf::Model model; + model.SetName("model1"); + EXPECT_TRUE(world.AddModel(model)); + EXPECT_EQ(1u, world.ModelCount()); + EXPECT_FALSE(world.AddModel(model)); + EXPECT_EQ(1u, world.ModelCount()); + + world.ClearModels(); + EXPECT_EQ(0u, world.ModelCount()); + + EXPECT_TRUE(world.AddModel(model)); + EXPECT_EQ(1u, world.ModelCount()); + const sdf::Model *modelFromWorld = world.ModelByIndex(0); + ASSERT_NE(nullptr, modelFromWorld); + EXPECT_EQ(modelFromWorld->Name(), model.Name()); +} + +///////////////////////////////////////////////// +TEST(DOMWorld, AddActor) +{ + sdf::World world; + EXPECT_EQ(0u, world.ActorCount()); + + sdf::Actor actor; + actor.SetName("actor1"); + EXPECT_TRUE(world.AddActor(actor)); + EXPECT_EQ(1u, world.ActorCount()); + EXPECT_FALSE(world.AddActor(actor)); + EXPECT_EQ(1u, world.ActorCount()); + + world.ClearActors(); + EXPECT_EQ(0u, world.ActorCount()); + + EXPECT_TRUE(world.AddActor(actor)); + EXPECT_EQ(1u, world.ActorCount()); + const sdf::Actor *actorFromWorld = world.ActorByIndex(0); + ASSERT_NE(nullptr, actorFromWorld); + EXPECT_EQ(actorFromWorld->Name(), actor.Name()); +} + +///////////////////////////////////////////////// +TEST(DOMWorld, AddLight) +{ + sdf::World world; + EXPECT_EQ(0u, world.LightCount()); + + sdf::Light light; + light.SetName("light1"); + EXPECT_TRUE(world.AddLight(light)); + EXPECT_EQ(1u, world.LightCount()); + EXPECT_FALSE(world.AddLight(light)); + EXPECT_EQ(1u, world.LightCount()); + + world.ClearLights(); + EXPECT_EQ(0u, world.LightCount()); + + EXPECT_TRUE(world.AddLight(light)); + EXPECT_EQ(1u, world.LightCount()); + const sdf::Light *lightFromWorld = world.LightByIndex(0); + ASSERT_NE(nullptr, lightFromWorld); + EXPECT_EQ(lightFromWorld->Name(), light.Name()); +} From 3d8147b89c6c17bd6fe531fdada00570c504a0bf Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 1 Dec 2021 16:10:03 -0800 Subject: [PATCH 45/60] prepare 12.3.0 (#766) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- Changelog.md | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a813b647f..2ffb5d2d0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ project (sdformat12) set (SDF_PROTOCOL_VERSION 1.9) set (SDF_MAJOR_VERSION 12) -set (SDF_MINOR_VERSION 2) +set (SDF_MINOR_VERSION 3) set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) diff --git a/Changelog.md b/Changelog.md index cbb98f660..1902878c4 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,13 @@ ## libsdformat 12.X +### libsdformat 12.3.0 (2021-12-01) + +1. Fix empty pose parsing fail for rotation_format='quat_xyzw' + * [Pull request #729](https://github.com/ignitionrobotics/sdformat/pull/729) + +1. Added Add & Clear function to World, Model, and Link. + * [Pull request #765](https://github.com/ignitionrobotics/sdformat/pull/765) + ### libsdformat 12.2.0 (2021-11-23) 1. Convert Joint DOM to Element. From 3b385c989cb1afd9fdaf844835a1ea52a77b8355 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 6 Dec 2021 10:34:24 -0800 Subject: [PATCH 46/60] Added ToElement conversion for physics and atmosphere (#771) * Added ToElement conversion for physics and atmosphere Signed-off-by: Nate Koenig * spelling Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/Atmosphere.hh | 5 +++++ include/sdf/Physics.hh | 17 +++++++++++++++++ src/Atmosphere.cc | 15 +++++++++++++++ src/Atmosphere_TEST.cc | 23 +++++++++++++++++++++++ src/Physics.cc | 38 ++++++++++++++++++++++++++++++++++++++ src/Physics_TEST.cc | 26 ++++++++++++++++++++++++++ 6 files changed, 124 insertions(+) diff --git a/include/sdf/Atmosphere.hh b/include/sdf/Atmosphere.hh index 3cee963ba..aaa9d5683 100644 --- a/include/sdf/Atmosphere.hh +++ b/include/sdf/Atmosphere.hh @@ -95,6 +95,11 @@ namespace sdf /// \return True if this instance equals the given atmosphere. public: bool operator==(const Atmosphere &_atmosphere); + /// \brief Create and return an SDF element filled with data from this + /// atmosphere. + /// \return SDF element pointer with updated atmosphere values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Physics.hh b/include/sdf/Physics.hh index 58331113b..c065cad66 100644 --- a/include/sdf/Physics.hh +++ b/include/sdf/Physics.hh @@ -102,6 +102,23 @@ namespace sdf /// \param[in] _factor The target real time factor. public: void SetRealTimeFactor(const double _factor); + /// \brief Get the maximum number of contacts allowed between two + /// entities. This value can be overridden by a max_contacts element in a + /// collision element. + /// \return Maximum number of contacts. + public: int MaxContacts() const; + + /// \brief Set the maximum number of contacts allowed between two + /// entities. This value can be overridden by a max_contacts element in a + /// collision element. + /// \param[in] _maxContacts Maximum number of contacts. + public: void SetMaxContacts(int _maxContacts); + + /// \brief Create and return an SDF element filled with data from this + /// physics. + /// \return SDF element pointer with updated physics values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/src/Atmosphere.cc b/src/Atmosphere.cc index ffe284387..36ca6d96a 100644 --- a/src/Atmosphere.cc +++ b/src/Atmosphere.cc @@ -18,6 +18,7 @@ #include #include #include "sdf/Atmosphere.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -144,3 +145,17 @@ bool Atmosphere::operator==(const Atmosphere &_atmosphere) ignition::math::equal(this->dataPtr->pressure, _atmosphere.dataPtr->pressure); } + +///////////////////////////////////////////////// +sdf::ElementPtr Atmosphere::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("atmosphere.sdf", elem); + + elem->GetAttribute("type")->Set("adiabatic"); + elem->GetElement("temperature")->Set(this->Temperature().Kelvin()); + elem->GetElement("pressure")->Set(this->Pressure()); + elem->GetElement("temperature_gradient")->Set(this->TemperatureGradient()); + + return elem; +} diff --git a/src/Atmosphere_TEST.cc b/src/Atmosphere_TEST.cc index eb9d6636b..37d11de3b 100644 --- a/src/Atmosphere_TEST.cc +++ b/src/Atmosphere_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include "sdf/World.hh" @@ -130,3 +131,25 @@ TEST(DOMAtmosphere, CopyAssignmentAfterMove) EXPECT_DOUBLE_EQ(200.0, atmosphere1.Temperature().Kelvin()); EXPECT_DOUBLE_EQ(100.0, atmosphere2.Temperature().Kelvin()); } + +///////////////////////////////////////////////// +TEST(DOMAtmosphere, ToElement) +{ + sdf::Atmosphere atmosphere; + atmosphere.SetType(sdf::AtmosphereType::ADIABATIC); + atmosphere.SetTemperature(ignition::math::Temperature(123)); + atmosphere.SetTemperatureGradient(1.34); + atmosphere.SetPressure(2.65); + + sdf::ElementPtr elem = atmosphere.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Atmosphere atmosphere2; + atmosphere2.Load(elem); + + // verify values after loading the element back + EXPECT_EQ(atmosphere.Temperature(), atmosphere2.Temperature()); + EXPECT_DOUBLE_EQ(atmosphere.TemperatureGradient(), + atmosphere2.TemperatureGradient()); + EXPECT_DOUBLE_EQ(atmosphere.Pressure(), atmosphere2.Pressure()); +} diff --git a/src/Physics.cc b/src/Physics.cc index 480b85407..643e9329a 100644 --- a/src/Physics.cc +++ b/src/Physics.cc @@ -16,6 +16,7 @@ */ #include +#include "sdf/parser.hh" #include "sdf/Physics.hh" #include "Utils.hh" @@ -41,6 +42,9 @@ class sdf::Physics::Implementation /// \brief Desired realtime factor. public: double rtf {1.0}; + + /// \brief Maximum number of contacts allowed between two entities. + public: int maxContacts{20}; }; ///////////////////////////////////////////////// @@ -113,6 +117,9 @@ Errors Physics::Load(sdf::ElementPtr _sdf) } this->dataPtr->rtf = doublePair.first; + this->dataPtr->maxContacts = + _sdf->Get("max_contacts", this->dataPtr->maxContacts).first; + return errors; } @@ -181,3 +188,34 @@ void Physics::SetRealTimeFactor(const double _factor) { this->dataPtr->rtf = _factor; } + +///////////////////////////////////////////////// +int Physics::MaxContacts() const +{ + return this->dataPtr->maxContacts; +} + +///////////////////////////////////////////////// +void Physics::SetMaxContacts(int _maxContacts) +{ + this->dataPtr->maxContacts = _maxContacts; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Physics::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("physics.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + elem->GetAttribute("default")->Set(this->IsDefault()); + elem->GetAttribute("type")->Set(this->EngineType()); + + elem->GetElement("max_step_size")->Set(this->MaxStepSize()); + elem->GetElement("real_time_factor")->Set(this->RealTimeFactor()); + elem->GetElement("max_contacts")->Set(this->MaxContacts()); + + /// \todo(nkoenig) Support engine specific parameters. + + return elem; +} diff --git a/src/Physics_TEST.cc b/src/Physics_TEST.cc index c02fcbc21..43bf95fa0 100644 --- a/src/Physics_TEST.cc +++ b/src/Physics_TEST.cc @@ -105,3 +105,29 @@ TEST(DOMPhysics, CopyAssignmentAfterMove) EXPECT_EQ("physics2", physics1.Name()); EXPECT_EQ("physics1", physics2.Name()); } + +///////////////////////////////////////////////// +TEST(DOMPhysics, ToElement) +{ + sdf::Physics physics; + physics.SetName("my-bullet-engine"); + physics.SetDefault(true); + physics.SetEngineType("bullet"); + physics.SetMaxStepSize(0.1); + physics.SetRealTimeFactor(20.4); + physics.SetMaxContacts(42); + + sdf::ElementPtr elem = physics.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Physics physics2; + physics2.Load(elem); + + // verify values after loading the element back + EXPECT_EQ(physics.Name(), physics2.Name()); + EXPECT_EQ(physics.IsDefault(), physics2.IsDefault()); + EXPECT_EQ(physics.EngineType(), physics2.EngineType()); + EXPECT_DOUBLE_EQ(physics.MaxStepSize(), physics2.MaxStepSize()); + EXPECT_DOUBLE_EQ(physics.RealTimeFactor(), physics2.RealTimeFactor()); + EXPECT_EQ(physics.MaxContacts(), physics2.MaxContacts()); +} From 520b74fbf2390246e4547c0ec782ab582c78eeba Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 6 Dec 2021 11:33:48 -0800 Subject: [PATCH 47/60] Aded ToElement conversion for shapes (#772) * Aded ToElement conversion for shapes Signed-off-by: Nate Koenig * doxygen Signed-off-by: Ian Chen Co-authored-by: Nate Koenig Co-authored-by: Ian Chen --- include/sdf/Box.hh | 5 ++++ include/sdf/Capsule.hh | 5 ++++ include/sdf/Cylinder.hh | 5 ++++ include/sdf/Ellipsoid.hh | 5 ++++ include/sdf/Heightmap.hh | 9 ++++++-- include/sdf/Mesh.hh | 5 ++++ include/sdf/Plane.hh | 5 ++++ include/sdf/Sphere.hh | 5 ++++ src/Box.cc | 13 +++++++++++ src/Box_TEST.cc | 16 +++++++++++++ src/Capsule.cc | 16 +++++++++++++ src/Capsule_TEST.cc | 18 +++++++++++++++ src/Cylinder.cc | 16 +++++++++++++ src/Cylinder_TEST.cc | 18 +++++++++++++++ src/Ellipsoid.cc | 13 +++++++++++ src/Ellipsoid_TEST.cc | 16 +++++++++++++ src/Heightmap.cc | 47 +++++++++++++++++++++++++++++++++++++ src/Heightmap_TEST.cc | 50 ++++++++++++++++++++++++++++++++++++++++ src/Mesh.cc | 30 ++++++++++++++++++++++++ src/Mesh_TEST.cc | 22 ++++++++++++++++++ src/Plane.cc | 16 +++++++++++++ src/Plane_TEST.cc | 18 +++++++++++++++ src/Sphere.cc | 13 +++++++++++ src/Sphere_TEST.cc | 16 +++++++++++++ 24 files changed, 380 insertions(+), 2 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 9ac7fa219..2ba1ee9fa 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -65,6 +65,11 @@ namespace sdf /// \return A reference to an ignition::math::Boxd object. public: ignition::math::Boxd &Shape(); + /// \brief Create and return an SDF element filled with data from this + /// box. + /// \return SDF element pointer with updated box values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index 76a91f077..56ffbe6b3 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -72,6 +72,11 @@ namespace sdf /// \return A reference to an ignition::math::Capsuled object. public: ignition::math::Capsuled &Shape(); + /// \brief Create and return an SDF element filled with data from this + /// capsule. + /// \return SDF element pointer with updated capsule values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 9a48b59a5..606399b21 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -72,6 +72,11 @@ namespace sdf /// \return A reference to an ignition::math::Cylinderd object. public: ignition::math::Cylinderd &Shape(); + /// \brief Create and return an SDF element filled with data from this + /// cylinder. + /// \return SDF element pointer with updated cylinder values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index fadd4ed81..fa18e9eed 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -64,6 +64,11 @@ namespace sdf /// \return A reference to an ignition::math::Ellipsoidd object. public: ignition::math::Ellipsoidd &Shape(); + /// \brief Create and return an SDF element filled with data from this + /// ellipsoid. + /// \return SDF element pointer with updated ellipsoid values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Heightmap.hh b/include/sdf/Heightmap.hh index 11a664b56..a08a1706d 100644 --- a/include/sdf/Heightmap.hh +++ b/include/sdf/Heightmap.hh @@ -47,8 +47,8 @@ namespace sdf public: double Size() const; /// \brief Set the size of the texture in meters. - /// \param[in] _uri The size of the texture in meters. - public: void SetSize(double _uri); + /// \param[in] _size The size of the texture in meters. + public: void SetSize(double _size); /// \brief Get the heightmap texture's diffuse map. /// \return The diffuse map of the heightmap texture. @@ -214,6 +214,11 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Create and return an SDF element filled with data from this + /// heightmap. + /// \return SDF element pointer with updated heightmap values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index b75349200..7636534d3 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -97,6 +97,11 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Create and return an SDF element filled with data from this + /// mesh. + /// \return SDF element pointer with updated mesh values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Plane.hh b/include/sdf/Plane.hh index c0ea5a4d7..e02de22e1 100644 --- a/include/sdf/Plane.hh +++ b/include/sdf/Plane.hh @@ -80,6 +80,11 @@ namespace sdf /// \return A reference to an ignition::math::Planed object. public: ignition::math::Planed &Shape(); + /// \brief Create and return an SDF element filled with data from this + /// plane. + /// \return SDF element pointer with updated plane values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 10d91b897..42f8da4c5 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -65,6 +65,11 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Create and return an SDF element filled with data from this + /// sphere. + /// \return SDF element pointer with updated sphere values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/src/Box.cc b/src/Box.cc index 90ceb26e3..6c029dac0 100644 --- a/src/Box.cc +++ b/src/Box.cc @@ -16,6 +16,7 @@ */ #include #include "sdf/Box.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -112,3 +113,15 @@ ignition::math::Boxd &Box::Shape() { return this->dataPtr->box; } + +///////////////////////////////////////////////// +sdf::ElementPtr Box::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("box_shape.sdf", elem); + + sdf::ElementPtr sizeElem = elem->GetElement("size"); + sizeElem->Set(this->Size()); + + return elem; +} diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index b77bd23a7..762d0a0b9 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -143,3 +143,19 @@ TEST(DOMBox, Shape) box.Shape().SetSize(ignition::math::Vector3d(1, 2, 3)); EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), box.Size()); } + +///////////////////////////////////////////////// +TEST(DOMBox, ToElement) +{ + sdf::Box box; + + box.SetSize(ignition::math::Vector3d(1, 2, 3)); + + sdf::ElementPtr elem = box.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Box box2; + box2.Load(elem); + + EXPECT_EQ(box.Size(), box2.Size()); +} diff --git a/src/Capsule.cc b/src/Capsule.cc index 507cbe0c4..a99159d69 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -16,6 +16,7 @@ */ #include #include "sdf/Capsule.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -134,3 +135,18 @@ ignition::math::Capsuled &Capsule::Shape() { return this->dataPtr->capsule; } + +///////////////////////////////////////////////// +sdf::ElementPtr Capsule::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("capsule_shape.sdf", elem); + + sdf::ElementPtr radiusElem = elem->GetElement("radius"); + radiusElem->Set(this->Radius()); + + sdf::ElementPtr lengthElem = elem->GetElement("length"); + lengthElem->Set(this->Length()); + + return elem; +} diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index 20b55c644..9c66d9797 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -179,3 +179,21 @@ TEST(DOMCapsule, Shape) EXPECT_DOUBLE_EQ(0.123, capsule.Radius()); EXPECT_DOUBLE_EQ(0.456, capsule.Length()); } + +///////////////////////////////////////////////// +TEST(DOMCapsule, ToElement) +{ + sdf::Capsule capsule; + + capsule.SetRadius(1.2); + capsule.SetLength(0.5); + + sdf::ElementPtr elem = capsule.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Capsule capsule2; + capsule2.Load(elem); + + EXPECT_DOUBLE_EQ(capsule.Radius(), capsule2.Radius()); + EXPECT_DOUBLE_EQ(capsule.Length(), capsule2.Length()); +} diff --git a/src/Cylinder.cc b/src/Cylinder.cc index c04de1592..681ea8440 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -16,6 +16,7 @@ */ #include #include "sdf/Cylinder.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -134,3 +135,18 @@ ignition::math::Cylinderd &Cylinder::Shape() { return this->dataPtr->cylinder; } + +///////////////////////////////////////////////// +sdf::ElementPtr Cylinder::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("cylinder_shape.sdf", elem); + + sdf::ElementPtr radiusElem = elem->GetElement("radius"); + radiusElem->Set(this->Radius()); + + sdf::ElementPtr lengthElem = elem->GetElement("length"); + lengthElem->Set(this->Length()); + + return elem; +} diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index 741dc77d4..7493f6f8b 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -175,3 +175,21 @@ TEST(DOMCylinder, Shape) EXPECT_DOUBLE_EQ(0.123, cylinder.Radius()); EXPECT_DOUBLE_EQ(0.456, cylinder.Length()); } + +///////////////////////////////////////////////// +TEST(DOMCylinder, ToElement) +{ + sdf::Cylinder cylinder; + + cylinder.SetRadius(1.2); + cylinder.SetLength(0.5); + + sdf::ElementPtr elem = cylinder.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Cylinder cylinder2; + cylinder2.Load(elem); + + EXPECT_DOUBLE_EQ(cylinder.Radius(), cylinder2.Radius()); + EXPECT_DOUBLE_EQ(cylinder.Length(), cylinder2.Length()); +} diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 694b9a1e1..8ef41b49a 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -16,6 +16,7 @@ */ #include #include "sdf/Ellipsoid.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -113,3 +114,15 @@ ignition::math::Ellipsoidd &Ellipsoid::Shape() { return this->dataPtr->ellipsoid; } + +///////////////////////////////////////////////// +sdf::ElementPtr Ellipsoid::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("ellipsoid_shape.sdf", elem); + + sdf::ElementPtr radiiElem = elem->GetElement("radii"); + radiiElem->Set(this->Radii()); + + return elem; +} diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc index 8a4bf4138..d05556f68 100644 --- a/src/Ellipsoid_TEST.cc +++ b/src/Ellipsoid_TEST.cc @@ -139,3 +139,19 @@ TEST(DOMEllipsoid, Shape) ellipsoid.Shape().SetRadii(expectedRadii); EXPECT_EQ(expectedRadii, ellipsoid.Radii()); } + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, ToElement) +{ + sdf::Ellipsoid ellipsoid; + + ellipsoid.SetRadii(ignition::math::Vector3d(0.1, 1.2, 3.4)); + + sdf::ElementPtr elem = ellipsoid.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Ellipsoid ellipsoid2; + ellipsoid2.Load(elem); + + EXPECT_EQ(ellipsoid.Radii(), ellipsoid2.Radii()); +} diff --git a/src/Heightmap.cc b/src/Heightmap.cc index 8d018730f..9583192f1 100644 --- a/src/Heightmap.cc +++ b/src/Heightmap.cc @@ -19,6 +19,7 @@ #include "Utils.hh" #include "sdf/Heightmap.hh" +#include "sdf/parser.hh" using namespace sdf; @@ -459,3 +460,49 @@ void Heightmap::AddBlend(const HeightmapBlend &_blend) { this->dataPtr->blends.push_back(_blend); } + +///////////////////////////////////////////////// +sdf::ElementPtr Heightmap::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("heightmap_shape.sdf", elem); + + // Uri + sdf::ElementPtr uriElem = elem->GetElement("uri"); + uriElem->Set(this->Uri()); + + // Size + sdf::ElementPtr sizeElem = elem->GetElement("size"); + sizeElem->Set(this->Size()); + + // Position + sdf::ElementPtr posElem = elem->GetElement("pos"); + posElem->Set(this->Position()); + + // Terrain paging + sdf::ElementPtr pagingElem = elem->GetElement("use_terrain_paging"); + pagingElem->Set(this->UseTerrainPaging()); + + // Sampling + sdf::ElementPtr samplingElem = elem->GetElement("sampling"); + samplingElem->Set(this->Sampling()); + + // Textures + for (const HeightmapTexture &tex : this->dataPtr->textures) + { + sdf::ElementPtr texElem = elem->AddElement("texture"); + texElem->GetElement("size")->Set(tex.Size()); + texElem->GetElement("diffuse")->Set(tex.Diffuse()); + texElem->GetElement("normal")->Set(tex.Normal()); + } + + // Blends + for (const HeightmapBlend &blend : this->dataPtr->blends) + { + sdf::ElementPtr blendElem = elem->AddElement("blend"); + blendElem->GetElement("min_height")->Set(blend.MinHeight()); + blendElem->GetElement("fade_dist")->Set(blend.FadeDistance()); + } + + return elem; +} diff --git a/src/Heightmap_TEST.cc b/src/Heightmap_TEST.cc index 4cd1b1dc9..fe45184a9 100644 --- a/src/Heightmap_TEST.cc +++ b/src/Heightmap_TEST.cc @@ -406,3 +406,53 @@ TEST(DOMHeightmap, LoadErrors) "missing a ")); EXPECT_NE(nullptr, heightmapBlend.Element()); } + +///////////////////////////////////////////////// +TEST(DOMHeightmap, ToElement) +{ + sdf::Heightmap heightmap; + + heightmap.SetUri("https://test-uri.org"); + heightmap.SetSize(ignition::math::Vector3d(1, 2, 3)); + heightmap.SetPosition(ignition::math::Vector3d(4, 5, 6)); + heightmap.SetUseTerrainPaging(true); + heightmap.SetSampling(2); + + sdf::HeightmapTexture texture; + texture.SetSize(1.2); + texture.SetDiffuse("diffuse_map"); + texture.SetNormal("normal_map"); + heightmap.AddTexture(texture); + + sdf::HeightmapBlend blend; + blend.SetMinHeight(1.2); + blend.SetFadeDistance(3.4); + heightmap.AddBlend(blend); + + sdf::ElementPtr elem = heightmap.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Heightmap heightmap2; + heightmap2.Load(elem); + + EXPECT_EQ(heightmap.Uri(), heightmap2.Uri()); + EXPECT_EQ(heightmap.Size(), heightmap2.Size()); + EXPECT_EQ(heightmap.Position(), heightmap2.Position()); + EXPECT_EQ(heightmap.UseTerrainPaging(), heightmap2.UseTerrainPaging()); + EXPECT_EQ(heightmap.Sampling(), heightmap2.Sampling()); + EXPECT_EQ(heightmap.TextureCount(), heightmap2.TextureCount()); + ASSERT_EQ(1u, heightmap2.TextureCount()); + EXPECT_DOUBLE_EQ(heightmap.TextureByIndex(0)->Size(), + heightmap2.TextureByIndex(0)->Size()); + EXPECT_EQ(heightmap.TextureByIndex(0)->Diffuse(), + heightmap2.TextureByIndex(0)->Diffuse()); + EXPECT_EQ(heightmap.TextureByIndex(0)->Normal(), + heightmap2.TextureByIndex(0)->Normal()); + + EXPECT_EQ(heightmap.BlendCount(), heightmap2.BlendCount()); + ASSERT_EQ(1u, heightmap2.BlendCount()); + EXPECT_DOUBLE_EQ(heightmap.BlendByIndex(0)->MinHeight(), + heightmap2.BlendByIndex(0)->MinHeight()); + EXPECT_DOUBLE_EQ(heightmap.BlendByIndex(0)->FadeDistance(), + heightmap2.BlendByIndex(0)->FadeDistance()); +} diff --git a/src/Mesh.cc b/src/Mesh.cc index 553623c3f..b517b98c0 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include "sdf/parser.hh" #include "sdf/Mesh.hh" using namespace sdf; @@ -176,3 +177,32 @@ void Mesh::SetCenterSubmesh(const bool _center) { this->dataPtr->centerSubmesh = _center; } + +///////////////////////////////////////////////// +sdf::ElementPtr Mesh::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("mesh_shape.sdf", elem); + + // Uri + sdf::ElementPtr uriElem = elem->GetElement("uri"); + uriElem->Set(this->Uri()); + + // Submesh + if (!this->dataPtr->submesh.empty()) + { + sdf::ElementPtr subMeshElem = elem->GetElement("submesh"); + + sdf::ElementPtr subMeshNameElem = subMeshElem->GetElement("name"); + subMeshNameElem->Set(this->dataPtr->submesh); + + sdf::ElementPtr subMeshCenterElem = subMeshElem->GetElement("center"); + subMeshCenterElem->Set(this->dataPtr->centerSubmesh); + } + + // Scale + sdf::ElementPtr scaleElem = elem->GetElement("scale"); + scaleElem->Set(this->Scale()); + + return elem; +} diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 41ecfe2c6..12a448a42 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -179,3 +179,25 @@ TEST(DOMMesh, Load) EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")); EXPECT_NE(nullptr, mesh.Element()); } + +///////////////////////////////////////////////// +TEST(DOMMesh, ToElement) +{ + sdf::Mesh mesh; + + mesh.SetUri("mesh-uri"); + mesh.SetScale(ignition::math::Vector3d(1, 2, 3)); + mesh.SetSubmesh("submesh"); + mesh.SetCenterSubmesh(false); + + sdf::ElementPtr elem = mesh.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Mesh mesh2; + mesh2.Load(elem); + + EXPECT_EQ(mesh.Uri(), mesh2.Uri()); + EXPECT_EQ(mesh.Scale(), mesh2.Scale()); + EXPECT_EQ(mesh.Submesh(), mesh2.Submesh()); + EXPECT_EQ(mesh.CenterSubmesh(), mesh2.CenterSubmesh()); +} diff --git a/src/Plane.cc b/src/Plane.cc index a24542080..408f72161 100644 --- a/src/Plane.cc +++ b/src/Plane.cc @@ -16,6 +16,7 @@ */ #include #include +#include "sdf/parser.hh" #include "sdf/Plane.hh" using namespace sdf; @@ -150,3 +151,18 @@ ignition::math::Planed &Plane::Shape() { return this->dataPtr->plane; } + +///////////////////////////////////////////////// +sdf::ElementPtr Plane::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("plane_shape.sdf", elem); + + sdf::ElementPtr normalElem = elem->GetElement("normal"); + normalElem->Set(this->Normal()); + + sdf::ElementPtr sizeElem = elem->GetElement("size"); + sizeElem->Set(this->Size()); + + return elem; +} diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 97fd5f780..04aa0be11 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -167,3 +167,21 @@ TEST(DOMPlane, Shape) plane.Shape().Offset()); EXPECT_EQ(ignition::math::Vector2d(1, 2), plane.Size()); } + +///////////////////////////////////////////////// +TEST(DOMPlane, ToElement) +{ + sdf::Plane plane; + + plane.SetNormal(ignition::math::Vector3d(0, 1, 0)); + plane.SetSize(ignition::math::Vector2d(2, 4)); + + sdf::ElementPtr elem = plane.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Plane plane2; + plane2.Load(elem); + + EXPECT_EQ(plane.Normal(), plane2.Normal()); + EXPECT_EQ(plane.Size(), plane2.Size()); +} diff --git a/src/Sphere.cc b/src/Sphere.cc index e1c8ad3ed..df666c58b 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include "sdf/parser.hh" #include "sdf/Sphere.hh" using namespace sdf; @@ -111,3 +112,15 @@ sdf::ElementPtr Sphere::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +sdf::ElementPtr Sphere::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("sphere_shape.sdf", elem); + + sdf::ElementPtr radiusElem = elem->GetElement("radius"); + radiusElem->Set(this->Radius()); + + return elem; +} diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 107ccac36..5e8d0407d 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -133,3 +133,19 @@ TEST(DOMSphere, Shape) sphere.Shape().SetRadius(0.123); EXPECT_DOUBLE_EQ(0.123, sphere.Radius()); } + +///////////////////////////////////////////////// +TEST(DOMSphere, ToElement) +{ + sdf::Sphere sphere; + + sphere.SetRadius(1.2); + + sdf::ElementPtr elem = sphere.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Sphere sphere2; + sphere2.Load(elem); + + EXPECT_DOUBLE_EQ(sphere.Radius(), sphere2.Radius()); +} From 8de1eb6fd49da8434e01fd94af26f8d7c452acd4 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 7 Dec 2021 17:02:10 -0800 Subject: [PATCH 48/60] Material toelement (#775) * Working on material DOM ToElement Signed-off-by: Nate Koenig * Update material ToElement Signed-off-by: Nate Koenig * Use floats Signed-off-by: Nate Koenig * Fix build Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/Material.hh | 12 ++++- src/Material.cc | 107 +++++++++++++++++++++++++++++++++++- src/Material_TEST.cc | 116 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 232 insertions(+), 3 deletions(-) diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index 0e1b3b8f1..68760de4f 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -103,12 +103,15 @@ namespace sdf /// \param[in] _color Emissive color. public: void SetEmissive(const ignition::math::Color &_color); - /// \brief Get render order + /// \brief Get render order for coplanar polygons. The higher value will + /// be rendered on top of the other coplanar polygons. The default value + /// is zero. /// \return Render order public: float RenderOrder() const; - /// \brief Set render order + /// \brief Set render order. /// \param[in] _renderOrder render order + /// \sa float RenderOrder() const public: void SetRenderOrder(const float _renderOrder); /// \brief Get whether dynamic lighting is enabled. The default @@ -190,6 +193,11 @@ namespace sdf /// \paramp[in] _filePath Full path to the file on disk. public: void SetFilePath(const std::string &_filePath); + /// \brief Create and return an SDF element filled with data from this + /// material. + /// \return SDF element pointer with updated material values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/src/Material.cc b/src/Material.cc index d4459801b..5c050c10a 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -19,9 +19,10 @@ #include #include -#include "sdf/Types.hh" #include "sdf/Material.hh" +#include "sdf/parser.hh" #include "sdf/Pbr.hh" +#include "sdf/Types.hh" #include "Utils.hh" using namespace sdf; @@ -358,3 +359,107 @@ void Material::SetFilePath(const std::string &_filePath) { this->dataPtr->filePath = _filePath; } + +///////////////////////////////////////////////// +sdf::ElementPtr Material::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("material.sdf", elem); + + elem->GetElement("ambient")->Set(this->Ambient()); + elem->GetElement("diffuse")->Set(this->Diffuse()); + elem->GetElement("specular")->Set(this->Specular()); + elem->GetElement("emissive")->Set(this->Emissive()); + elem->GetElement("render_order")->Set(this->RenderOrder()); + elem->GetElement("lighting")->Set(this->Lighting()); + elem->GetElement("double_sided")->Set(this->DoubleSided()); + + // Script, if set + if (!this->ScriptName().empty() && !this->ScriptUri().empty()) + { + sdf::ElementPtr scriptElem = elem->GetElement("script"); + scriptElem->GetElement("uri")->Set(this->ScriptUri()); + scriptElem->GetElement("name")->Set(this->ScriptName()); + } + + // Shader properties + sdf::ElementPtr shaderElem = elem->GetElement("shader"); + switch (this->dataPtr->shader) + { + default: + case ShaderType::PIXEL: + shaderElem->GetAttribute("type")->Set("pixel"); + break; + case ShaderType::VERTEX: + shaderElem->GetAttribute("type")->Set("vertex"); + break; + case ShaderType::NORMAL_MAP_OBJECTSPACE: + shaderElem->GetAttribute("type")->Set("normal_map_object_space"); + break; + case ShaderType::NORMAL_MAP_TANGENTSPACE: + shaderElem->GetAttribute("type")->Set("normal_map_tangent_space"); + break; + } + if (!this->NormalMap().empty()) + shaderElem->GetElement("normal_map")->Set(this->NormalMap()); + + // PBR material + if (this->dataPtr->pbr) + { + const PbrWorkflow *workflow = this->dataPtr->pbr->Workflow( + PbrWorkflowType::METAL); + sdf::ElementPtr pbrElem = elem->GetElement("pbr"); + if (workflow && workflow->Type() == PbrWorkflowType::METAL) + { + sdf::ElementPtr metalElem = pbrElem->GetElement("metal"); + metalElem->GetElement("albedo_map")->Set(workflow->AlbedoMap()); + metalElem->GetElement("roughness_map")->Set(workflow->RoughnessMap()); + metalElem->GetElement("roughness")->Set(workflow->Roughness()); + metalElem->GetElement("metalness_map")->Set(workflow->MetalnessMap()); + metalElem->GetElement("metalness")->Set(workflow->Metalness()); + metalElem->GetElement("ambient_occlusion_map")->Set( + workflow->AmbientOcclusionMap()); + sdf::ElementPtr normalElem = metalElem->GetElement("normal_map"); + if (workflow->NormalMapType() == NormalMapSpace::TANGENT) + normalElem->GetAttribute("type")->Set("tangent"); + else + normalElem->GetAttribute("type")->Set("object"); + normalElem->Set(workflow->NormalMap()); + + metalElem->GetElement("emissive_map")->Set(workflow->EmissiveMap()); + + sdf::ElementPtr lightElem = metalElem->GetElement("light_map"); + lightElem->GetAttribute("uv_set")->Set(workflow->LightMapTexCoordSet()); + lightElem->Set(workflow->LightMap()); + } + + workflow = this->dataPtr->pbr->Workflow(PbrWorkflowType::SPECULAR); + if (workflow && workflow->Type() == PbrWorkflowType::SPECULAR) + { + sdf::ElementPtr specularElem = pbrElem->GetElement("specular"); + specularElem->GetElement("albedo_map")->Set(workflow->AlbedoMap()); + specularElem->GetElement("specular_map")->Set(workflow->SpecularMap()); + specularElem->GetElement("environment_map")->Set( + workflow->EnvironmentMap()); + specularElem->GetElement("ambient_occlusion_map")->Set( + workflow->AmbientOcclusionMap()); + specularElem->GetElement("emissive_map")->Set(workflow->EmissiveMap()); + specularElem->GetElement("glossiness_map")->Set( + workflow->GlossinessMap()); + specularElem->GetElement("glossiness")->Set(workflow->Glossiness()); + + sdf::ElementPtr normalElem = specularElem->GetElement("normal_map"); + if (workflow->NormalMapType() == NormalMapSpace::TANGENT) + normalElem->GetAttribute("type")->Set("tangent"); + else + normalElem->GetAttribute("type")->Set("object"); + normalElem->Set(workflow->NormalMap()); + + sdf::ElementPtr lightElem = specularElem->GetElement("light_map"); + lightElem->GetAttribute("uv_set")->Set(workflow->LightMapTexCoordSet()); + lightElem->Set(workflow->LightMap()); + } + } + + return elem; +} diff --git a/src/Material_TEST.cc b/src/Material_TEST.cc index 115a56849..c67c4a81a 100644 --- a/src/Material_TEST.cc +++ b/src/Material_TEST.cc @@ -289,3 +289,119 @@ TEST(DOMMaterial, InvalidSdf) sdf::Errors errors = material.Load(elem); EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); } + +///////////////////////////////////////////////// +TEST(DOMMaterial, ToElement) +{ + sdf::Material material; + + material.SetAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f, 1.0f)); + material.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.5f)); + material.SetSpecular(ignition::math::Color(0.6f, 0.4f, 0.8f, 0.8f)); + material.SetEmissive(ignition::math::Color(0.2f, 0.7f, 0.9f, 0.1f)); + + material.SetRenderOrder(0.5); + material.SetLighting(false); + material.SetDoubleSided(true); + material.SetScriptUri("my-uri"); + material.SetScriptName("my-script-name"); + material.SetShader(sdf::ShaderType::VERTEX); + material.SetNormalMap("my-normal-map"); + + sdf::PbrWorkflow workflow; + workflow.SetAlbedoMap("albedo"); + workflow.SetNormalMap("normal"); + workflow.SetEnvironmentMap("env"); + workflow.SetAmbientOcclusionMap("ambient"); + workflow.SetRoughnessMap("rough"); + workflow.SetMetalnessMap("metalness"); + workflow.SetEmissiveMap("emissive"); + workflow.SetGlossinessMap("gloss"); + workflow.SetSpecularMap("gloss"); + workflow.SetLightMap("light", 1u); + workflow.SetMetalness(1.0); + workflow.SetRoughness(0.1); + + // Testing using METAL workflow + { + sdf::Pbr pbr; + workflow.SetType(sdf::PbrWorkflowType::METAL); + pbr.SetWorkflow(sdf::PbrWorkflowType::METAL, workflow); + material.SetPbrMaterial(pbr); + + sdf::ElementPtr elem = material.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Material material2; + material2.Load(elem); + + EXPECT_EQ(material.Ambient(), material2.Ambient()); + EXPECT_EQ(material.Diffuse(), material2.Diffuse()); + EXPECT_EQ(material.Specular(), material2.Specular()); + EXPECT_EQ(material.Emissive(), material2.Emissive()); + EXPECT_DOUBLE_EQ(material.RenderOrder(), material2.RenderOrder()); + EXPECT_EQ(material.Lighting(), material2.Lighting()); + EXPECT_EQ(material.DoubleSided(), material2.DoubleSided()); + EXPECT_EQ(material.ScriptUri(), material2.ScriptUri()); + EXPECT_EQ(material.ScriptName(), material2.ScriptName()); + EXPECT_EQ(material.Shader(), material2.Shader()); + EXPECT_EQ(material.NormalMap(), material2.NormalMap()); + + const sdf::Pbr *pbr2 = material2.PbrMaterial(); + const sdf::PbrWorkflow *flow1 = pbr.Workflow(sdf::PbrWorkflowType::METAL); + const sdf::PbrWorkflow *flow2 = pbr2->Workflow(sdf::PbrWorkflowType::METAL); + EXPECT_EQ(flow1->AlbedoMap(), flow2->AlbedoMap()); + EXPECT_EQ(flow1->NormalMap(), flow2->NormalMap()); + EXPECT_EQ(flow1->AmbientOcclusionMap(), + flow2->AmbientOcclusionMap()); + EXPECT_EQ(flow1->RoughnessMap(), flow2->RoughnessMap()); + EXPECT_EQ(flow1->MetalnessMap(), flow2->MetalnessMap()); + EXPECT_EQ(flow1->EmissiveMap(), flow2->EmissiveMap()); + EXPECT_EQ(flow1->LightMap(), flow2->LightMap()); + EXPECT_DOUBLE_EQ(flow1->Metalness(), flow2->Metalness()); + EXPECT_DOUBLE_EQ(flow1->Roughness(), flow2->Roughness()); + EXPECT_EQ(flow1->Type(), flow2->Type()); + } + + // Testing using SPECULAR workflow + { + sdf::Pbr pbr; + workflow.SetType(sdf::PbrWorkflowType::SPECULAR); + pbr.SetWorkflow(sdf::PbrWorkflowType::SPECULAR, workflow); + material.SetPbrMaterial(pbr); + + sdf::ElementPtr elem = material.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Material material2; + material2.Load(elem); + + EXPECT_EQ(material.Ambient(), material2.Ambient()); + EXPECT_EQ(material.Diffuse(), material2.Diffuse()); + EXPECT_EQ(material.Specular(), material2.Specular()); + EXPECT_EQ(material.Emissive(), material2.Emissive()); + EXPECT_DOUBLE_EQ(material.RenderOrder(), material2.RenderOrder()); + EXPECT_EQ(material.Lighting(), material2.Lighting()); + EXPECT_EQ(material.DoubleSided(), material2.DoubleSided()); + EXPECT_EQ(material.ScriptUri(), material2.ScriptUri()); + EXPECT_EQ(material.ScriptName(), material2.ScriptName()); + EXPECT_EQ(material.Shader(), material2.Shader()); + EXPECT_EQ(material.NormalMap(), material2.NormalMap()); + + const sdf::Pbr *pbr2 = material2.PbrMaterial(); + const sdf::PbrWorkflow *flow1 = + pbr.Workflow(sdf::PbrWorkflowType::SPECULAR); + const sdf::PbrWorkflow *flow2 = + pbr2->Workflow(sdf::PbrWorkflowType::SPECULAR); + EXPECT_EQ(flow1->AlbedoMap(), flow2->AlbedoMap()); + EXPECT_EQ(flow1->NormalMap(), flow2->NormalMap()); + EXPECT_EQ(flow1->EnvironmentMap(), flow2->EnvironmentMap()); + EXPECT_EQ(flow1->AmbientOcclusionMap(), + flow2->AmbientOcclusionMap()); + EXPECT_EQ(flow1->EmissiveMap(), flow2->EmissiveMap()); + EXPECT_EQ(flow1->GlossinessMap(), flow2->GlossinessMap()); + EXPECT_EQ(flow1->SpecularMap(), flow2->SpecularMap()); + EXPECT_EQ(flow1->LightMap(), flow2->LightMap()); + EXPECT_EQ(flow1->Type(), flow2->Type()); + } +} From b28967c31abe2ed9570a93947a49f0bab952b976 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 7 Dec 2021 20:42:36 -0800 Subject: [PATCH 49/60] Added Geometry ToElement function (#776) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Louise Poubel --- include/sdf/Geometry.hh | 5 + src/Geometry.cc | 42 ++++++++ src/Geometry_TEST.cc | 205 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 252 insertions(+) diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 6223a1737..b3d0a3b02 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -189,6 +189,11 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Create and return an SDF element filled with data from this + /// geometry. + /// \return SDF element pointer with updated geometry values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/src/Geometry.cc b/src/Geometry.cc index 59cacd0f0..d2c933356 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -24,6 +24,7 @@ #include "sdf/Ellipsoid.hh" #include "sdf/Heightmap.hh" #include "sdf/Mesh.hh" +#include "sdf/parser.hh" #include "sdf/Plane.hh" #include "sdf/Sphere.hh" @@ -270,3 +271,44 @@ sdf::ElementPtr Geometry::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +sdf::ElementPtr Geometry::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("geometry.sdf", elem); + + switch (this->dataPtr->type) + { + case GeometryType::BOX: + elem->InsertElement(this->dataPtr->box->ToElement()); + break; + case GeometryType::CYLINDER: + elem->InsertElement(this->dataPtr->cylinder->ToElement()); + break; + case GeometryType::PLANE: + elem->InsertElement(this->dataPtr->plane->ToElement()); + break; + case GeometryType::SPHERE: + elem->InsertElement(this->dataPtr->sphere->ToElement()); + break; + case GeometryType::MESH: + elem->InsertElement(this->dataPtr->mesh->ToElement()); + break; + case GeometryType::HEIGHTMAP: + elem->InsertElement(this->dataPtr->heightmap->ToElement()); + break; + case GeometryType::CAPSULE: + elem->InsertElement(this->dataPtr->capsule->ToElement()); + break; + case GeometryType::ELLIPSOID: + elem->InsertElement(this->dataPtr->ellipsoid->ToElement()); + break; + case GeometryType::EMPTY: + default: + elem->AddElement("empty"); + break; + } + + return elem; +} diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 6aaab1a2a..0d3809a01 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -21,6 +21,7 @@ #include "sdf/Cylinder.hh" #include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" +#include "sdf/Heightmap.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" #include "sdf/Sphere.hh" @@ -259,3 +260,207 @@ TEST(DOMGeometry, Plane) EXPECT_EQ(ignition::math::Vector3d::UnitX, geom.PlaneShape()->Normal()); EXPECT_EQ(ignition::math::Vector2d(9, 8), geom.PlaneShape()->Size()); } + +///////////////////////////////////////////////// +TEST(DOMGeometry, ToElement) +{ + // Box + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::BOX); + sdf::Box box; + geom.SetBoxShape(box); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_NE(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + } + + // Capsule + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::CAPSULE); + sdf::Capsule capsule; + geom.SetCapsuleShape(capsule); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_NE(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + } + + // Cylinder + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::CYLINDER); + sdf::Cylinder cylinder; + geom.SetCylinderShape(cylinder); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_NE(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + } + + // Ellipsoid + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::ELLIPSOID); + sdf::Ellipsoid ellipsoid; + geom.SetEllipsoidShape(ellipsoid); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_NE(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + } + + // Sphere + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::SPHERE); + sdf::Sphere sphere; + geom.SetSphereShape(sphere); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_NE(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + } + + // Plane + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::PLANE); + sdf::Plane plane; + geom.SetPlaneShape(plane); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_NE(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + } + + // Mesh + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::MESH); + sdf::Mesh mesh; + geom.SetMeshShape(mesh); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_NE(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + } + + // Heightmap + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::HEIGHTMAP); + sdf::Heightmap heightmap; + geom.SetHeightmapShape(heightmap); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_NE(nullptr, geom2.HeightmapShape()); + } +} From e8a73e43eb64a2f3542de797bfadcae370e76f7e Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 9 Dec 2021 12:08:09 -0800 Subject: [PATCH 50/60] Added ToElement conversion for Collision, Surface, and Visual (#777) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added ToElement conversion for Collision, Surface, and Visual Signed-off-by: Nate Koenig * Update src/Visual_TEST.cc Co-authored-by: Alejandro Hernández Cordero Co-authored-by: Nate Koenig Co-authored-by: Alejandro Hernández Cordero --- include/sdf/Collision.hh | 5 +++++ include/sdf/Surface.hh | 5 +++++ include/sdf/Visual.hh | 5 +++++ src/Collision.cc | 27 +++++++++++++++++++++++++++ src/Collision_TEST.cc | 32 ++++++++++++++++++++++++++++++++ src/Surface.cc | 16 ++++++++++++++++ src/Visual.cc | 40 +++++++++++++++++++++++++++++++++++++--- src/Visual_TEST.cc | 35 +++++++++++++++++++++++++++++++++++ 8 files changed, 162 insertions(+), 3 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 342985ff7..5a12eede2 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -115,6 +115,11 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Create and return an SDF element filled with data from this + /// collision. + /// \return SDF element pointer with updated collision values. + public: sdf::ElementPtr ToElement() const; + /// \brief Give the name of the xml parent of this object, to be used /// for resolving poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index dd39aea94..6312ffbd3 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -87,6 +87,11 @@ namespace sdf /// \param[in] _cont The contact object. public: void SetContact(const sdf::Contact &_contact); + /// \brief Create and return an SDF element filled with data from this + /// surface. + /// \return SDF element pointer with updated surface values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index fce9e6de2..a3f00fb58 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -160,6 +160,11 @@ namespace sdf /// \param[in] _laserRetro The lidar reflective intensity. public: void SetLaserRetro(double _laserRetro); + /// \brief Create and return an SDF element filled with data from this + /// visual. + /// \return SDF element pointer with updated visual values. + public: sdf::ElementPtr ToElement() const; + /// \brief Give the name of the xml parent of this object, to be used /// for resolving poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. diff --git a/src/Collision.cc b/src/Collision.cc index da67eb5e4..cbeabaf9b 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -20,6 +20,7 @@ #include "sdf/Collision.hh" #include "sdf/Error.hh" #include "sdf/Geometry.hh" +#include "sdf/parser.hh" #include "sdf/Surface.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" @@ -197,3 +198,29 @@ sdf::ElementPtr Collision::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +sdf::ElementPtr Collision::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("collision.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + + // Set pose + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + // Set the geometry + elem->InsertElement(this->dataPtr->geom.ToElement()); + + // Set the surface + elem->InsertElement(this->dataPtr->surface.ToElement()); + + return elem; +} diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 33b49267d..bdf15ff18 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -168,3 +168,35 @@ TEST(DOMcollision, SetSurface) ASSERT_NE(nullptr, collision.Surface()->Contact()); EXPECT_EQ(collision.Surface()->Contact()->CollideBitmask(), 0x2); } + +///////////////////////////////////////////////// +TEST(DOMCollision, ToElement) +{ + sdf::Collision collision; + + collision.SetName("my-collision"); + + sdf::Geometry geom; + collision.SetGeom(geom); + collision.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + + sdf::Surface surface; + sdf::Contact contact; + contact.SetCollideBitmask(123u); + surface.SetContact(contact); + collision.SetSurface(surface); + + sdf::ElementPtr elem = collision.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Collision collision2; + collision2.Load(elem); + const sdf::Surface *surface2 = collision2.Surface(); + + EXPECT_EQ(collision.Name(), collision2.Name()); + EXPECT_EQ(collision.RawPose(), collision2.RawPose()); + EXPECT_NE(nullptr, collision2.Geom()); + ASSERT_NE(nullptr, surface2); + ASSERT_NE(nullptr, surface2->Contact()); + EXPECT_EQ(123u, surface2->Contact()->CollideBitmask()); +} diff --git a/src/Surface.cc b/src/Surface.cc index 7e7283906..3977cf3fa 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -16,6 +16,7 @@ */ #include "sdf/Element.hh" +#include "sdf/parser.hh" #include "sdf/Surface.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -79,6 +80,7 @@ Errors Contact::Load(ElementPtr _sdf) static_cast(_sdf->Get("collide_bitmask")); } + // \todo(nkoenig) Parse the remaining collide properties. return errors; } ///////////////////////////////////////////////// @@ -137,6 +139,7 @@ Errors Surface::Load(ElementPtr _sdf) errors.insert(errors.end(), err.begin(), err.end()); } + // \todo(nkoenig) Parse the remaining surface properties. return errors; } ///////////////////////////////////////////////// @@ -156,3 +159,16 @@ void Surface::SetContact(const sdf::Contact &_contact) { this->dataPtr->contact = _contact; } + +///////////////////////////////////////////////// +sdf::ElementPtr Surface::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("surface.sdf", elem); + + sdf::ElementPtr contactElem = elem->GetElement("contact"); + contactElem->GetElement("collide_bitmask")->Set( + this->dataPtr->contact.CollideBitmask()); + + return elem; +} diff --git a/src/Visual.cc b/src/Visual.cc index 6b6fe5e22..4421f66c8 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -17,12 +17,13 @@ #include #include #include +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "sdf/Error.hh" +#include "sdf/Geometry.hh" +#include "sdf/parser.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" -#include "sdf/Geometry.hh" -#include "FrameSemantics.hh" -#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -300,3 +301,36 @@ void Visual::SetPoseRelativeToGraph( { this->dataPtr->poseRelativeToGraph = _graph; } + +///////////////////////////////////////////////// +sdf::ElementPtr Visual::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("visual.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + + // Set pose + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + // Set the geometry + elem->InsertElement(this->dataPtr->geom.ToElement()); + + elem->GetElement("cast_shadows")->Set(this->CastShadows()); + elem->GetElement("laser_retro")->Set(this->LaserRetro()); + elem->GetElement("transparency")->Set(this->Transparency()); + elem->GetElement("visibility_flags")->Set(this->VisibilityFlags()); + + if (this->dataPtr->material) + { + elem->InsertElement(this->dataPtr->material->ToElement()); + } + + return elem; +} diff --git a/src/Visual_TEST.cc b/src/Visual_TEST.cc index 836abf3a8..6119416b7 100644 --- a/src/Visual_TEST.cc +++ b/src/Visual_TEST.cc @@ -285,3 +285,38 @@ TEST(DOMVisual, SetLaserRetro) EXPECT_TRUE(visual.HasLaserRetro()); EXPECT_DOUBLE_EQ(150, visual.LaserRetro()); } + +///////////////////////////////////////////////// +TEST(DOMVisual, ToElement) +{ + sdf::Visual visual; + visual.SetName("my-visual"); + visual.SetCastShadows(true); + visual.SetTransparency(0.2f); + visual.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + visual.SetVisibilityFlags(1234u); + visual.SetHasLaserRetro(true); + visual.SetLaserRetro(1.2); + + sdf::Geometry geom; + visual.SetGeom(geom); + + sdf::Material mat; + visual.SetMaterial(mat); + + sdf::ElementPtr elem = visual.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Visual visual2; + visual2.Load(elem); + + EXPECT_EQ(visual.Name(), visual2.Name()); + EXPECT_EQ(visual.CastShadows(), visual2.CastShadows()); + EXPECT_DOUBLE_EQ(visual.Transparency(), visual2.Transparency()); + EXPECT_EQ(visual.RawPose(), visual2.RawPose()); + EXPECT_EQ(visual.VisibilityFlags(), visual2.VisibilityFlags()); + EXPECT_EQ(visual.HasLaserRetro(), visual2.HasLaserRetro()); + EXPECT_DOUBLE_EQ(visual.LaserRetro(), visual2.LaserRetro()); + EXPECT_NE(nullptr, visual2.Geom()); + EXPECT_NE(nullptr, visual2.Material()); +} From 93c045cda0b167ff226ec5940740ccb415225f5e Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Thu, 9 Dec 2021 14:16:10 -0800 Subject: [PATCH 51/60] PrintConfig option to preserve includes when converting to string (#749) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Jenn Nguyen Co-authored-by: Marco A. Gutiérrez Co-authored-by: Addisu Z. Taddese --- include/sdf/PrintConfig.hh | 31 +++-- src/CMakeLists.txt | 1 + src/Element.cc | 19 +-- src/PrintConfig.cc | 15 +- src/PrintConfig_TEST.cc | 32 +++++ src/cmd/cmdsdformat.rb.in | 33 ++++- src/ign.cc | 30 ++++ test/integration/CMakeLists.txt | 1 + test/integration/interface_api.cc | 35 +++++ test/integration/print_config.cc | 223 ++++++++++++++++++++++++++++++ 10 files changed, 393 insertions(+), 27 deletions(-) create mode 100644 src/PrintConfig_TEST.cc create mode 100644 test/integration/print_config.cc diff --git a/include/sdf/PrintConfig.hh b/include/sdf/PrintConfig.hh index d9d3d507c..2e3b700de 100644 --- a/include/sdf/PrintConfig.hh +++ b/include/sdf/PrintConfig.hh @@ -24,20 +24,27 @@ namespace sdf { -inline namespace SDF_VERSION_NAMESPACE -{ + inline namespace SDF_VERSION_NAMESPACE { -/// This class contains configuration options for printing elements. -class SDFORMAT_VISIBLE PrintConfig -{ - /// \brief Default constructor. - public: PrintConfig(); + /// This class contains configuration options for printing elements. + class SDFORMAT_VISIBLE PrintConfig + { + /// \brief Default constructor. + public: PrintConfig(); - /// \brief Private data pointer. - IGN_UTILS_IMPL_PTR(dataPtr) -}; + /// \brief Set print config to preserve tags. + /// \param[in] _preserve True to preserve tags. + /// False to expand included model. + public: void SetPreserveIncludes(bool _preserve); -} -} + /// \brief Check if tags are to be preserved or expanded. + /// \return True if tags are preserved. + /// False if they are to be expanded. + public: bool PreserveIncludes() const; + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } +} #endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 1e3c67eb2..a36fe8eda 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -134,6 +134,7 @@ if (BUILD_SDF_TEST) ParticleEmitter_TEST.cc Pbr_TEST.cc Physics_TEST.cc + PrintConfig_TEST.cc Plane_TEST.cc Root_TEST.cc Scene_TEST.cc diff --git a/src/Element.cc b/src/Element.cc index 091c7df24..d9e710868 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -505,7 +505,11 @@ void Element::PrintValuesImpl(const std::string &_prefix, const PrintConfig &_config, std::ostringstream &_out) const { - if (this->GetExplicitlySetInFile() || _includeDefaultElements) + if (_config.PreserveIncludes() && this->GetIncludeElement() != nullptr) + { + _out << this->GetIncludeElement()->ToString(_prefix, _config); + } + else if (this->GetExplicitlySetInFile() || _includeDefaultElements) { _out << _prefix << "<" << this->dataPtr->name; @@ -533,14 +537,11 @@ void Element::PrintValuesImpl(const std::string &_prefix, for (eiter = this->dataPtr->elements.begin(); eiter != this->dataPtr->elements.end(); ++eiter) { - if ((*eiter)->GetExplicitlySetInFile() || _includeDefaultElements) - { - (*eiter)->ToString(_prefix + " ", - _includeDefaultElements, - _includeDefaultAttributes, - _config, - _out); - } + (*eiter)->ToString(_prefix + " ", + _includeDefaultElements, + _includeDefaultAttributes, + _config, + _out); } _out << _prefix << "dataPtr->name << ">\n"; } diff --git a/src/PrintConfig.cc b/src/PrintConfig.cc index fb9f41999..3770f5007 100644 --- a/src/PrintConfig.cc +++ b/src/PrintConfig.cc @@ -24,7 +24,8 @@ inline namespace SDF_VERSION_NAMESPACE ///////////////////////////////////////////////// class PrintConfig::Implementation { - + /// \brief True to preserve tags, false to expand. + public: bool preserveIncludes = false; }; ///////////////////////////////////////////////// @@ -33,5 +34,17 @@ PrintConfig::PrintConfig() { } +///////////////////////////////////////////////// +void PrintConfig::SetPreserveIncludes(bool _preserve) +{ + this->dataPtr->preserveIncludes = _preserve; +} + +///////////////////////////////////////////////// +bool PrintConfig::PreserveIncludes() const +{ + return this->dataPtr->preserveIncludes; +} + } } diff --git a/src/PrintConfig_TEST.cc b/src/PrintConfig_TEST.cc new file mode 100644 index 000000000..ff473bf5d --- /dev/null +++ b/src/PrintConfig_TEST.cc @@ -0,0 +1,32 @@ +/* + * Copyright 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include "sdf/PrintConfig.hh" +#include "test_config.h" + +///////////////////////////////////////////////// +TEST(PrintConfig, PreserveIncludes) +{ + sdf::PrintConfig config; + EXPECT_FALSE(config.PreserveIncludes()); + config.SetPreserveIncludes(true); + EXPECT_TRUE(config.PreserveIncludes()); + config.SetPreserveIncludes(false); + EXPECT_FALSE(config.PreserveIncludes()); +} diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 52130611d..9d4d757c4 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -43,6 +43,7 @@ COMMANDS = { 'sdf' => " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph. (WARNING: This is for advanced\n" + " use only and the output may change without any promise of stability)\n" + " -p [ --print ] arg Print converted arg.\n" + + " -i [ --preserve-includes ] Preserve included tags when printing converted arg (does not preserve merge-includes).\n" + COMMON_OPTIONS } @@ -75,9 +76,11 @@ class Cmd opts.on('-d', '--describe [VERSION]', 'Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@)') do |v| options['describe'] = v end - opts.on('-p arg', '--print arg', String, - 'Print converted arg') do |arg| - options['print'] = arg + opts.on('-p', '--print', 'Print converted arg') do + options['print'] = 1 + end + opts.on('-i', '--preserve-includes', 'Preserve included tags when printing converted arg (does not preserve merge-includes)') do + options['preserve_includes'] = 1 end opts.on('-g arg', '--graph type', String, 'Print PoseRelativeTo or FrameAttachedTo graph') do |graph_type| @@ -101,6 +104,21 @@ class Cmd options['command'] = ARGV[0] + if options['preserve_includes'] and not options['print'] + puts usage + exit(-1) + end + + if options['print'] + filename = args.pop + if filename + options['print'] = filename + else + puts usage + exit(-1) + end + end + options end @@ -160,8 +178,13 @@ class Cmd Importer.extern 'int cmdDescribe(const char *)' exit(Importer.cmdDescribe(options['describe'])) elsif options.key?('print') - Importer.extern 'int cmdPrint(const char *)' - exit(Importer.cmdPrint(File.expand_path(options['print']))) + if options['preserve_includes'] + Importer.extern 'int cmdPrintPreserveIncludes(const char *)' + exit(Importer.cmdPrintPreserveIncludes(File.expand_path(options['print']))) + else + Importer.extern 'int cmdPrint(const char *)' + exit(Importer.cmdPrint(File.expand_path(options['print']))) + end elsif options.key?('graph') Importer.extern 'int cmdGraph(const char *, const char *)' exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1]))) diff --git a/src/ign.cc b/src/ign.cc index e046d6fc1..16cdb0d24 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -158,6 +158,36 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path) return 0; } +////////////////////////////////////////////////// +extern "C" SDFORMAT_VISIBLE int cmdPrintPreserveIncludes(const char *_path) +{ + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::SDFPtr sdf(new sdf::SDF()); + + if (!sdf::init(sdf)) + { + std::cerr << "Error: SDF schema initialization failed.\n"; + return -1; + } + + if (!sdf::readFile(_path, sdf)) + { + std::cerr << "Error: SDF parsing the xml failed.\n"; + return -1; + } + + sdf::PrintConfig config; + config.SetPreserveIncludes(true); + sdf->PrintValues(config); + + return 0; +} + ////////////////////////////////////////////////// // cppcheck-suppress unusedFunction extern "C" SDFORMAT_VISIBLE int cmdGraph( diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index bc2fb4481..43d786c6e 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -40,6 +40,7 @@ set(tests plugin_bool.cc plugin_include.cc pose_1_9_sdf.cc + print_config.cc provide_feedback.cc root_dom.cc scene_dom.cc diff --git a/test/integration/interface_api.cc b/test/integration/interface_api.cc index 05a60c2b9..badda1669 100644 --- a/test/integration/interface_api.cc +++ b/test/integration/interface_api.cc @@ -32,6 +32,7 @@ #include "sdf/InterfaceModel.hh" #include "sdf/Model.hh" #include "sdf/Param.hh" +#include "sdf/PrintConfig.hh" #include "sdf/Root.hh" #include "sdf/Types.hh" #include "sdf/World.hh" @@ -860,3 +861,37 @@ TEST_F(InterfaceAPI, NameCollision) EXPECT_EQ(sdf::ErrorCode::DUPLICATE_NAME, errors[0].Code()); } } + +///////////////////////////////////////////////// +// Tests PrintConfig +TEST_F(InterfaceAPI, TomlParserModelIncludePrintConfig) +{ + const std::string testFile = sdf::testing::TestFile( + "sdf", "model_include_with_interface_api.sdf"); + + this->config.RegisterCustomModelParser(customTomlParser); + sdf::Root root; + sdf::Errors errors = root.Load(testFile, this->config); + EXPECT_TRUE(errors.empty()) << errors; + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + const sdf::ElementPtr includeElem = model->Element()->GetFirstElement(); + ASSERT_NE(nullptr, includeElem); + + const std::string expectedIncludeStr = +R"( + double_pendulum.toml + 1 0 0 0 0 0 + + value1 + value2 + + +)"; + + sdf::PrintConfig printConfig; + EXPECT_EQ(includeElem->ToString("", printConfig), expectedIncludeStr); + printConfig.SetPreserveIncludes(true); + EXPECT_EQ(includeElem->ToString("", printConfig), expectedIncludeStr); +} diff --git a/test/integration/print_config.cc b/test/integration/print_config.cc new file mode 100644 index 000000000..a3cc9aba1 --- /dev/null +++ b/test/integration/print_config.cc @@ -0,0 +1,223 @@ +/* + * Copyright 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include + +#include "sdf/sdf.hh" + +#include "test_config.h" + +///////////////////////////////////////////////// +TEST(PrintConfig, PreserveIncludes) +{ + const std::string modelPath = sdf::testing::TestFile("integration", "model"); + + sdf::ParserConfig parserConfig; + parserConfig.SetFindCallback( + [&](const std::string &_file) + { + return sdf::filesystem::append(modelPath, _file); + }); + + const std::string includeStr = +R"( + box + test_box + 1 2 3 0 0 0 + link + + +)"; + + const std::string modelWithIncludeStr = +R"( + + test_model + test_model + 1 2 3 0 0 0 + link + + + +)"; + + const std::string sdfStr = + "" + " " + + includeStr + + modelWithIncludeStr + + " " + ""; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdfStr, parserConfig); + EXPECT_TRUE(errors.empty()) << errors; + + auto *world = root.WorldByIndex(0); + ASSERT_NE(world, nullptr); + auto *includedModel = world->ModelByIndex(0); + ASSERT_NE(includedModel, nullptr); + auto *modelWithInclude = world->ModelByIndex(1); + ASSERT_NE(modelWithInclude, nullptr); + + const std::string expandedIncludeStr = +R"( + 1 2 3 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + +)"; + + const std::string expandedModelWithIncludeStr = +R"( + + + + + + meshes/mesh.dae + + my_submesh +
true
+ + 0.1 0.2 0.3 + + + + + + + meshes/mesh.dae + + another_submesh +
false
+ + 1.2 2.3 3.4 + + + + + 1 2 3 0 0 0 + + + +)"; + + sdf::PrintConfig config; + // by default, included model should be expanded + EXPECT_EQ(includedModel->Element()->ToString("", config), expandedIncludeStr); + EXPECT_EQ(modelWithInclude->Element()->ToString("", config), + expandedModelWithIncludeStr); + + config.SetPreserveIncludes(true); + EXPECT_EQ(includedModel->Element()->ToString("", config), includeStr); + EXPECT_EQ(modelWithInclude->Element()->ToString("", config), + modelWithIncludeStr); +} + +///////////////////////////////////////////////// +// Test verifies preserving includes does not work for merge-includes. +// Need to update test if issue is addressed. +// https://github.com/ignitionrobotics/sdformat/issues/769 +TEST(PrintConfig, PreserveIncludesWithMerge) +{ + const std::string modelPath = sdf::testing::TestFile("integration", "model"); + + sdf::ParserConfig parserConfig; + parserConfig.SetFindCallback( + [&](const std::string &_file) + { + return sdf::filesystem::append(modelPath, _file); + }); + + const std::string includeMergeStr = +R"( + + box + test_box2 + + +)"; + + const std::string sdfStr = + "" + " " + + includeMergeStr + + " " + ""; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdfStr, parserConfig); + EXPECT_TRUE(errors.empty()) << errors; + + auto *world = root.WorldByIndex(0); + ASSERT_NE(world, nullptr); + auto *includeMergedModel = world->ModelByIndex(0); + ASSERT_NE(includeMergedModel, nullptr); + + const std::string expectedIncludeMerge = +R"( + + 0 0 0.5 0 -0 0 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0 0 0 0 0 0 + + +)"; + + sdf::PrintConfig config; + EXPECT_EQ(includeMergedModel->Element()->ToString("", config), + expectedIncludeMerge); + config.SetPreserveIncludes(true); + EXPECT_NE(includeMergedModel->Element()->ToString("", config), + includeMergeStr); + EXPECT_EQ(includeMergedModel->Element()->ToString("", config), + expectedIncludeMerge); +} From 5b48e41daca9264c25b568631fd713418543f694 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 9 Dec 2021 17:30:39 -0800 Subject: [PATCH 52/60] Added ToElement for ParticleEmitter and Link (#781) * Added ToElement for ParticleEmitter and Link Signed-off-by: Nate Koenig * expect double eq Signed-off-by: Ian Chen Co-authored-by: Nate Koenig Co-authored-by: Ian Chen --- include/sdf/Link.hh | 14 +++++ include/sdf/ParticleEmitter.hh | 5 ++ src/Link.cc | 84 ++++++++++++++++++++++++++ src/Link_TEST.cc | 106 +++++++++++++++++++++++++++++++++ src/ParticleEmitter.cc | 45 +++++++++++++- src/ParticleEmitter_TEST.cc | 51 ++++++++++++++++ 6 files changed, 304 insertions(+), 1 deletion(-) diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index eca492f77..55207c476 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -276,6 +276,12 @@ namespace sdf /// exists. public: bool AddSensor(const Sensor &_sensor); + /// \brief Add a particle emitter to the link. + /// \param[in] _emitter Particle emitter to add. + /// \return True if successful, false if a particle emitter with the name + /// already exists. + public: bool AddParticleEmitter(const ParticleEmitter &_sensor); + /// \brief Remove all collisions public: void ClearCollisions(); @@ -288,6 +294,14 @@ namespace sdf /// \brief Remove all sensors public: void ClearSensors(); + /// \brief Remove all particle emitters + public: void ClearParticleEmitters(); + + /// \brief Create and return an SDF element filled with data from this + /// link. + /// \return SDF element pointer with updated link values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/ParticleEmitter.hh b/include/sdf/ParticleEmitter.hh index d15eaa639..95b68b202 100644 --- a/include/sdf/ParticleEmitter.hh +++ b/include/sdf/ParticleEmitter.hh @@ -316,6 +316,11 @@ namespace sdf /// \paramp[in] _filePath Full path to the file on disk. public: void SetFilePath(const std::string &_filePath); + /// \brief Create and return an SDF element filled with data from this + /// particle emitter. + /// \return SDF element pointer with updated particle emitter values. + public: sdf::ElementPtr ToElement() const; + /// \brief Set the name of the xml parent of this object, to be used /// for resolving poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. diff --git a/src/Link.cc b/src/Link.cc index 7a5ed5846..14ff6fe10 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -25,6 +25,7 @@ #include "sdf/Error.hh" #include "sdf/Light.hh" #include "sdf/Link.hh" +#include "sdf/parser.hh" #include "sdf/ParticleEmitter.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" @@ -534,6 +535,15 @@ bool Link::AddSensor(const Sensor &_sensor) return true; } +////////////////////////////////////////////////// +bool Link::AddParticleEmitter(const ParticleEmitter &_emitter) +{ + if (this->ParticleEmitterNameExists(_emitter.Name())) + return false; + this->dataPtr->emitters.push_back(_emitter); + return true; +} + ////////////////////////////////////////////////// void Link::ClearCollisions() { @@ -557,3 +567,77 @@ void Link::ClearSensors() { this->dataPtr->sensors.clear(); } + +////////////////////////////////////////////////// +void Link::ClearParticleEmitters() +{ + this->dataPtr->emitters.clear(); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Link::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("link.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + + // Set pose + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + // inertial + sdf::ElementPtr inertialElem = elem->GetElement("inertial"); + inertialElem->GetElement("pose")->Set( + this->dataPtr->inertial.Pose()); + const ignition::math::MassMatrix3d &massMatrix = + this->dataPtr->inertial.MassMatrix(); + inertialElem->GetElement("mass")->Set(massMatrix.Mass()); + sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); + inertiaElem->GetElement("ixx")->Set(massMatrix.Ixx()); + inertiaElem->GetElement("ixy")->Set(massMatrix.Ixy()); + inertiaElem->GetElement("ixz")->Set(massMatrix.Ixz()); + inertiaElem->GetElement("iyy")->Set(massMatrix.Iyy()); + inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz()); + inertiaElem->GetElement("izz")->Set(massMatrix.Izz()); + + // wind mode + elem->GetElement("enable_wind")->Set(this->EnableWind()); + + // Collisions + for (const sdf::Collision &collision : this->dataPtr->collisions) + { + elem->InsertElement(collision.ToElement()); + } + + // Light + for (const sdf::Light &light : this->dataPtr->lights) + { + elem->InsertElement(light.ToElement()); + } + + // Particle emitters + for (const sdf::ParticleEmitter &emitter : this->dataPtr->emitters) + { + elem->InsertElement(emitter.ToElement()); + } + + // Sensors + for (const sdf::Sensor &sensor : this->dataPtr->sensors) + { + elem->InsertElement(sensor.ToElement()); + } + + // Visuals + for (const sdf::Visual &visual : this->dataPtr->visuals) + { + elem->InsertElement(visual.ToElement()); + } + + return elem; +} diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 6dd29d8c5..dc80a479c 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -303,3 +303,109 @@ TEST(DOMLink, AddSensor) ASSERT_NE(nullptr, sensorFromLink); EXPECT_EQ(sensorFromLink->Name(), sensor.Name()); } + +///////////////////////////////////////////////// +TEST(DOMLink, ToElement) +{ + sdf::Link link; + link.SetName("my-name"); + + ignition::math::Inertiald inertial { + {2.3, + ignition::math::Vector3d(1.4, 2.3, 3.2), + ignition::math::Vector3d(0.1, 0.2, 0.3)}, + ignition::math::Pose3d(1, 2, 3, 0, 0, 0)}; + EXPECT_TRUE(link.SetInertial(inertial)); + link.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + link.SetEnableWind(true); + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 1; ++i) + { + sdf::Collision collision; + collision.SetName("collision" + std::to_string(i)); + EXPECT_TRUE(link.AddCollision(collision)); + EXPECT_FALSE(link.AddCollision(collision)); + } + link.ClearCollisions(); + } + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 2; i++) + { + sdf::Visual visual; + visual.SetName("visual" + std::to_string(i)); + EXPECT_TRUE(link.AddVisual(visual)); + EXPECT_FALSE(link.AddVisual(visual)); + } + link.ClearVisuals(); + } + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 3; i++) + { + sdf::Light light; + light.SetName("light" + std::to_string(i)); + EXPECT_TRUE(link.AddLight(light)); + EXPECT_FALSE(link.AddLight(light)); + } + link.ClearLights(); + } + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 4; i++) + { + sdf::Sensor sensor; + sensor.SetName("sensor" + std::to_string(i)); + EXPECT_TRUE(link.AddSensor(sensor)); + EXPECT_FALSE(link.AddSensor(sensor)); + } + link.ClearSensors(); + } + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 5; i++) + { + sdf::ParticleEmitter emitter; + emitter.SetName("emitter" + std::to_string(i)); + EXPECT_TRUE(link.AddParticleEmitter(emitter)); + EXPECT_FALSE(link.AddParticleEmitter(emitter)); + } + link.ClearParticleEmitters(); + } + + sdf::ElementPtr elem = link.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Link link2; + link2.Load(elem); + + EXPECT_EQ(link.Name(), link2.Name()); + EXPECT_EQ(link.Inertial(), link2.Inertial()); + EXPECT_EQ(link.RawPose(), link2.RawPose()); + EXPECT_EQ(link.EnableWind(), link2.EnableWind()); + EXPECT_EQ(link.CollisionCount(), link2.CollisionCount()); + for (uint64_t i = 0; i < link2.CollisionCount(); ++i) + EXPECT_NE(nullptr, link2.CollisionByIndex(i)); + + EXPECT_EQ(link.VisualCount(), link2.VisualCount()); + for (uint64_t i = 0; i < link2.VisualCount(); ++i) + EXPECT_NE(nullptr, link2.VisualByIndex(i)); + + EXPECT_EQ(link.LightCount(), link2.LightCount()); + for (uint64_t i = 0; i < link2.LightCount(); ++i) + EXPECT_NE(nullptr, link2.LightByIndex(i)); + + EXPECT_EQ(link.SensorCount(), link2.SensorCount()); + for (uint64_t i = 0; i < link2.SensorCount(); ++i) + EXPECT_NE(nullptr, link2.SensorByIndex(i)); + + EXPECT_EQ(link.ParticleEmitterCount(), link2.ParticleEmitterCount()); + for (uint64_t i = 0; i < link2.ParticleEmitterCount(); ++i) + EXPECT_NE(nullptr, link2.ParticleEmitterByIndex(i)); +} diff --git a/src/ParticleEmitter.cc b/src/ParticleEmitter.cc index f1e353a66..7bb6a83a9 100644 --- a/src/ParticleEmitter.cc +++ b/src/ParticleEmitter.cc @@ -19,8 +19,9 @@ #include #include -#include "sdf/ParticleEmitter.hh" #include "sdf/Error.hh" +#include "sdf/parser.hh" +#include "sdf/ParticleEmitter.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" @@ -515,3 +516,45 @@ void ParticleEmitter::SetPoseRelativeToGraph( { this->dataPtr->poseRelativeToGraph = _graph; } + +///////////////////////////////////////////////// +sdf::ElementPtr ParticleEmitter::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("particle_emitter.sdf", elem); + + // Set pose + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + elem->GetAttribute("name")->Set(this->Name()); + elem->GetAttribute("type")->Set(this->TypeStr()); + elem->GetElement("emitting")->Set(this->Emitting()); + elem->GetElement("duration")->Set(this->Duration()); + elem->GetElement("size")->Set(this->Size()); + elem->GetElement("particle_size")->Set(this->ParticleSize()); + elem->GetElement("lifetime")->Set(this->Lifetime()); + elem->GetElement("rate")->Set(this->Rate()); + elem->GetElement("min_velocity")->Set(this->MinVelocity()); + elem->GetElement("max_velocity")->Set(this->MaxVelocity()); + elem->GetElement("scale_rate")->Set(this->ScaleRate()); + elem->GetElement("color_start")->Set(this->ColorStart()); + elem->GetElement("color_end")->Set(this->ColorEnd()); + elem->GetElement("color_range_image")->Set(this->ColorRangeImage()); + elem->GetElement("topic")->Set(this->Topic()); + elem->GetElement("particle_scatter_ratio")->Set(this->ScatterRatio()); + + if (this->dataPtr->material) + { + elem->InsertElement(this->dataPtr->material->ToElement()); + } + + return elem; +} + + diff --git a/src/ParticleEmitter_TEST.cc b/src/ParticleEmitter_TEST.cc index 3cc03c85c..2ddb2b512 100644 --- a/src/ParticleEmitter_TEST.cc +++ b/src/ParticleEmitter_TEST.cc @@ -116,3 +116,54 @@ TEST(DOMParticleEmitter, Construction) emitter.SetPoseRelativeTo("/test/relative"); EXPECT_EQ("/test/relative", emitter.PoseRelativeTo()); } + +///////////////////////////////////////////////// +TEST(DOMParticleEmitter, ToElement) +{ + sdf::ParticleEmitter emitter; + + emitter.SetName("my-emitter"); + emitter.SetType(sdf::ParticleEmitterType::BOX); + emitter.SetEmitting(true); + emitter.SetDuration(1.2); + emitter.SetLifetime(3.4); + emitter.SetRate(12.5); + emitter.SetScaleRate(0.2); + emitter.SetMinVelocity(32.4); + emitter.SetMaxVelocity(50.1); + emitter.SetSize(ignition::math::Vector3d(1, 2, 3)); + emitter.SetParticleSize(ignition::math::Vector3d(4, 5, 6)); + emitter.SetColorStart(ignition::math::Color(0.1f, 0.2f, 0.3f, 1.0f)); + emitter.SetColorEnd(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0f)); + emitter.SetColorRangeImage("my-image"); + emitter.SetTopic("my-topic"); + emitter.SetScatterRatio(0.3f); + emitter.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + sdf::Material material; + emitter.SetMaterial(material); + + sdf::ElementPtr elem = emitter.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::ParticleEmitter emitter2; + emitter2.Load(elem); + + EXPECT_EQ(emitter.Name(), emitter2.Name()); + EXPECT_EQ(emitter.Type(), emitter2.Type()); + EXPECT_EQ(emitter.Emitting(), emitter2.Emitting()); + EXPECT_DOUBLE_EQ(emitter.Duration(), emitter2.Duration()); + EXPECT_DOUBLE_EQ(emitter.Lifetime(), emitter2.Lifetime()); + EXPECT_DOUBLE_EQ(emitter.Rate(), emitter2.Rate()); + EXPECT_DOUBLE_EQ(emitter.ScaleRate(), emitter2.ScaleRate()); + EXPECT_DOUBLE_EQ(emitter.MinVelocity(), emitter2.MinVelocity()); + EXPECT_DOUBLE_EQ(emitter.MaxVelocity(), emitter2.MaxVelocity()); + EXPECT_EQ(emitter.Size(), emitter2.Size()); + EXPECT_EQ(emitter.ParticleSize(), emitter2.ParticleSize()); + EXPECT_EQ(emitter.ColorStart(), emitter2.ColorStart()); + EXPECT_EQ(emitter.ColorEnd(), emitter2.ColorEnd()); + EXPECT_EQ(emitter.ColorRangeImage(), emitter2.ColorRangeImage()); + EXPECT_EQ(emitter.Topic(), emitter2.Topic()); + EXPECT_FLOAT_EQ(emitter.ScatterRatio(), emitter2.ScatterRatio()); + EXPECT_EQ(emitter.RawPose(), emitter2.RawPose()); + EXPECT_NE(nullptr, emitter2.Material()); +} From 2f21fb24b9ab6d858ddaa5316d86424ae0f0df55 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 10 Dec 2021 08:10:56 -0800 Subject: [PATCH 53/60] Model actor toelement functions (#782) * Adding toelement for model and actor Signed-off-by: Nate Koenig * Updated tests Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/Actor.hh | 23 ++++++++++ include/sdf/Model.hh | 5 +++ src/Actor.cc | 102 +++++++++++++++++++++++++++++++++++++++++++ src/Actor_TEST.cc | 95 ++++++++++++++++++++++++++++++++++++++++ src/Model.cc | 48 ++++++++++++++++++++ src/Model_TEST.cc | 78 +++++++++++++++++++++++++++++++++ 6 files changed, 351 insertions(+) diff --git a/include/sdf/Actor.hh b/include/sdf/Actor.hh index d14cd04f1..a8a537b53 100644 --- a/include/sdf/Actor.hh +++ b/include/sdf/Actor.hh @@ -360,6 +360,29 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Add a link to the actor. + /// \param[in] _link Link to add. + /// \return True if successful, false if a link with the name already + /// exists. + public: bool AddLink(const Link &_link); + + /// \brief Add a joint to the actor. + /// \param[in] _link Joint to add. + /// \return True if successful, false if a joint with the name already + /// exists. + public: bool AddJoint(const Joint &_joint); + + /// \brief Remove all links. + public: void ClearLinks(); + + /// \brief Remove all joints. + public: void ClearJoints(); + + /// \brief Create and return an SDF element filled with data from this + /// actor. + /// \return SDF element pointer with updated actor values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index ac047de1d..75ed0420d 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -346,6 +346,11 @@ namespace sdf public: const NestedInclude *InterfaceModelNestedIncludeByIndex( const uint64_t _index) const; + /// \brief Create and return an SDF element filled with data from this + /// model. + /// \return SDF element pointer with updated model values. + public: sdf::ElementPtr ToElement() const; + /// \brief Add a link to the model. /// \param[in] _link Link to add. /// \return True if successful, false if a link with the name already diff --git a/src/Actor.cc b/src/Actor.cc index 2f10ae7b4..aaf43adb3 100644 --- a/src/Actor.cc +++ b/src/Actor.cc @@ -19,6 +19,7 @@ #include #include "sdf/Actor.hh" #include "sdf/Error.hh" +#include "sdf/parser.hh" #include "Utils.hh" using namespace sdf; @@ -681,3 +682,104 @@ void Actor::SetFilePath(const std::string &_filePath) { this->dataPtr->filePath = _filePath; } + +////////////////////////////////////////////////// +bool Actor::AddLink(const Link &_link) +{ + if (this->LinkNameExists(_link.Name())) + return false; + this->dataPtr->links.push_back(_link); + return true; +} + +////////////////////////////////////////////////// +bool Actor::AddJoint(const Joint &_joint) +{ + if (this->JointNameExists(_joint.Name())) + return false; + this->dataPtr->joints.push_back(_joint); + return true; +} + +////////////////////////////////////////////////// +void Actor::ClearLinks() +{ + this->dataPtr->links.clear(); +} + +////////////////////////////////////////////////// +void Actor::ClearJoints() +{ + this->dataPtr->joints.clear(); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Actor::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("actor.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + // Set pose + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + // Skin + if (this->dataPtr->skinFilename != "__default__") + { + sdf::ElementPtr skinElem = elem->GetElement("skin"); + skinElem->GetElement("filename")->Set(this->dataPtr->skinFilename); + skinElem->GetElement("scale")->Set(this->dataPtr->skinScale); + } + + // Script + sdf::ElementPtr scriptElem = elem->GetElement("script"); + scriptElem->GetElement("loop")->Set(this->ScriptLoop()); + scriptElem->GetElement("delay_start")->Set(this->ScriptDelayStart()); + scriptElem->GetElement("auto_start")->Set(this->ScriptAutoStart()); + // Trajectory for the script + for (const sdf::Trajectory &traj : this->dataPtr->trajectories) + { + sdf::ElementPtr trajElem = scriptElem->AddElement("trajectory"); + trajElem->GetAttribute("id")->Set(traj.Id()); + trajElem->GetAttribute("type")->Set(traj.Type()); + trajElem->GetAttribute("tension")->Set(traj.Tension()); + // Waypoints in the trajectory. + for (uint64_t i = 0; i < traj.WaypointCount(); ++i) + { + const Waypoint *wp = traj.WaypointByIndex(i); + if (wp) + { + sdf::ElementPtr wayElem = trajElem->AddElement("waypoint"); + wayElem->GetElement("time")->Set(wp->Time()); + wayElem->GetElement("pose")->Set(wp->Pose()); + } + } + } + + // Animations + for (const sdf::Animation &anim : this->dataPtr->animations) + { + sdf::ElementPtr animElem = elem->AddElement("animation"); + animElem->GetAttribute("name")->Set(anim.Name()); + animElem->GetElement("filename")->Set(anim.Filename()); + animElem->GetElement("scale")->Set(anim.Scale()); + animElem->GetElement("interpolate_x")->Set(anim.InterpolateX()); + } + + // Joints + for (const sdf::Joint &joint : this->dataPtr->joints) + elem->InsertElement(joint.ToElement()); + + // Link + for (const sdf::Link &link : this->dataPtr->links) + elem->InsertElement(link.ToElement()); + + return elem; +} + diff --git a/src/Actor_TEST.cc b/src/Actor_TEST.cc index bac434933..6b9ed9c03 100644 --- a/src/Actor_TEST.cc +++ b/src/Actor_TEST.cc @@ -502,3 +502,98 @@ TEST(DOMTrajectory, CopyAssignmentAfterMove) EXPECT_TRUE(TrajectoriesEqual(sdf::Trajectory(), trajectory1)); EXPECT_TRUE(TrajectoriesEqual(CreateDummyTrajectory(), trajectory2)); } + +///////////////////////////////////////////////// +TEST(DOMActor, ToElement) +{ + sdf::Actor actor; + + actor.SetName("my-actor"); + actor.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + actor.SetSkinFilename("my-skinfilename"); + actor.SetSkinScale(1.2); + actor.SetScriptLoop(true); + actor.SetScriptDelayStart(2.4); + actor.SetScriptAutoStart(true); + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 1; ++i) + { + sdf::Link link; + link.SetName("link" + std::to_string(i)); + EXPECT_TRUE(actor.AddLink(link)); + EXPECT_FALSE(actor.AddLink(link)); + } + actor.ClearLinks(); + } + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 2; ++i) + { + sdf::Joint joint; + joint.SetName("joint" + std::to_string(i)); + EXPECT_TRUE(actor.AddJoint(joint)); + EXPECT_FALSE(actor.AddJoint(joint)); + } + actor.ClearJoints(); + } + + sdf::Trajectory trajectory; + trajectory.SetId(1); + trajectory.SetType("type"); + trajectory.SetTension(1.0); + sdf::Waypoint waypointA, waypointB; + waypointA.SetTime(0.0); + waypointA.SetPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + trajectory.AddWaypoint(waypointA); + waypointB.SetTime(1.0); + waypointB.SetPose(ignition::math::Pose3d(2, 4, 6, 0.1, 0.2, 0.3)); + trajectory.AddWaypoint(waypointB); + actor.AddTrajectory(trajectory); + + sdf::Animation animation; + animation.SetName("my-animation"); + animation.SetFilename("my-filename"); + animation.SetScale(1.2); + animation.SetInterpolateX(true); + + sdf::ElementPtr elem = actor.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Actor actor2; + actor2.Load(elem); + + EXPECT_EQ(actor.Name(), actor2.Name()); + EXPECT_EQ(actor.RawPose(), actor2.RawPose()); + EXPECT_DOUBLE_EQ(actor.SkinScale(), actor2.SkinScale()); + EXPECT_EQ(actor.ScriptLoop(), actor2.ScriptLoop()); + EXPECT_DOUBLE_EQ(actor.ScriptDelayStart(), actor2.ScriptDelayStart()); + + EXPECT_EQ(actor.LinkCount(), actor2.LinkCount()); + for (uint64_t i = 0; i < actor2.LinkCount(); ++i) + EXPECT_NE(nullptr, actor2.LinkByIndex(i)); + + EXPECT_EQ(actor.JointCount(), actor2.JointCount()); + for (uint64_t i = 0; i < actor2.JointCount(); ++i) + EXPECT_NE(nullptr, actor2.JointByIndex(i)); + + ASSERT_EQ(1u, actor2.TrajectoryCount()); + const sdf::Trajectory *trajectory2 = actor2.TrajectoryByIndex(0u); + ASSERT_NE(nullptr, trajectory2); + EXPECT_EQ(trajectory.Id(), trajectory2->Id()); + EXPECT_EQ(trajectory.Type(), trajectory2->Type()); + EXPECT_DOUBLE_EQ(trajectory.Tension(), trajectory2->Tension()); + + ASSERT_EQ(2u, trajectory2->WaypointCount()); + const sdf::Waypoint *waypointA2 = trajectory2->WaypointByIndex(0u); + ASSERT_NE(nullptr, waypointA2); + EXPECT_DOUBLE_EQ(waypointA.Time(), waypointA2->Time()); + EXPECT_EQ(waypointA.Pose(), waypointA2->Pose()); + + const sdf::Waypoint *waypointB2 = trajectory2->WaypointByIndex(1u); + ASSERT_NE(nullptr, waypointB2); + EXPECT_DOUBLE_EQ(waypointB.Time(), waypointB2->Time()); + EXPECT_EQ(waypointB.Pose(), waypointB2->Pose()); +} diff --git a/src/Model.cc b/src/Model.cc index 361b23243..bf5810803 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -791,6 +791,54 @@ const NestedInclude *Model::InterfaceModelNestedIncludeByIndex( return nullptr; } +///////////////////////////////////////////////// +sdf::ElementPtr Model::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("model.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + + if (!this->dataPtr->canonicalLink.empty()) + { + elem->GetAttribute("canonical_link")->Set(this->dataPtr->canonicalLink); + } + + if (!this->dataPtr->placementFrameName.empty()) + { + elem->GetAttribute("placement_frame")->Set( + this->dataPtr->placementFrameName); + } + + elem->GetElement("static")->Set(this->Static()); + elem->GetElement("self_collide")->Set(this->SelfCollide()); + elem->GetElement("allow_auto_disable")->Set(this->AllowAutoDisable()); + elem->GetElement("enable_wind")->Set(this->EnableWind()); + + // Set pose + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + // Links + for (const sdf::Link &link : this->dataPtr->links) + elem->InsertElement(link.ToElement()); + + // Joints + for (const sdf::Joint &joint : this->dataPtr->joints) + elem->InsertElement(joint.ToElement()); + + // Model + for (const sdf::Model &model : this->dataPtr->models) + elem->InsertElement(model.ToElement()); + + return elem; +} + ////////////////////////////////////////////////// bool Model::AddLink(const Link &_link) { diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 6054d1fe7..537ebe0ba 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -282,3 +282,81 @@ TEST(DOMModel, AddModel) ASSERT_NE(nullptr, modelFromModel); EXPECT_EQ(modelFromModel->Name(), nestedModel.Name()); } + +///////////////////////////////////////////////// +TEST(DOMModel, ToElement) +{ + sdf::Model model; + + model.SetName("my-model"); + model.SetStatic(true); + model.SetSelfCollide(true); + model.SetAllowAutoDisable(true); + model.SetEnableWind(true); + model.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 1; ++i) + { + sdf::Link link; + link.SetName("link" + std::to_string(i)); + EXPECT_TRUE(model.AddLink(link)); + EXPECT_FALSE(model.AddLink(link)); + } + model.ClearLinks(); + } + model.SetCanonicalLinkName("link1"); + model.SetPlacementFrameName("link0"); + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 2; ++i) + { + sdf::Joint joint; + joint.SetName("joint" + std::to_string(i)); + EXPECT_TRUE(model.AddJoint(joint)); + EXPECT_FALSE(model.AddJoint(joint)); + } + model.ClearJoints(); + } + + for (int j = 0; j < 1; ++j) + { + for (int i = 0; i < 3; ++i) + { + sdf::Model nestedModel; + nestedModel.SetName("model" + std::to_string(i)); + EXPECT_TRUE(model.AddModel(nestedModel)); + EXPECT_FALSE(model.AddModel(nestedModel)); + } + model.ClearModels(); + } + + sdf::ElementPtr elem = model.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Model model2; + model2.Load(elem); + + EXPECT_EQ(model.Name(), model2.Name()); + EXPECT_EQ(model.Static(), model2.Static()); + EXPECT_EQ(model.SelfCollide(), model2.SelfCollide()); + EXPECT_EQ(model.AllowAutoDisable(), model2.AllowAutoDisable()); + EXPECT_EQ(model.EnableWind(), model2.EnableWind()); + EXPECT_EQ(model.RawPose(), model2.RawPose()); + EXPECT_EQ(model.CanonicalLinkName(), model2.CanonicalLinkName()); + EXPECT_EQ(model.PlacementFrameName(), model2.PlacementFrameName()); + + EXPECT_EQ(model.LinkCount(), model2.LinkCount()); + for (uint64_t i = 0; i < model2.LinkCount(); ++i) + EXPECT_NE(nullptr, model2.LinkByIndex(i)); + + EXPECT_EQ(model.JointCount(), model2.JointCount()); + for (uint64_t i = 0; i < model2.JointCount(); ++i) + EXPECT_NE(nullptr, model2.JointByIndex(i)); + + EXPECT_EQ(model.ModelCount(), model2.ModelCount()); + for (uint64_t i = 0; i < model2.ModelCount(); ++i) + EXPECT_NE(nullptr, model2.ModelByIndex(i)); +} From 826e7d62dafc303463af9991db7b917008e99004 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Sat, 11 Dec 2021 08:34:26 -0800 Subject: [PATCH 54/60] More to element (#783) * Added more ToElement functions Signed-off-by: Nate Koenig * Revert Link toelement changes Signed-off-by: Nate Koenig * Added toelement conversion for world, scene, sky, gui Signed-off-by: Nate Koenig * Fix element insertion, and updated tests Signed-off-by: Nate Koenig * Update docs Signed-off-by: Nate Koenig * Updates Signed-off-by: Nate Koenig * minor doxygen update Signed-off-by: Ian Chen Co-authored-by: Nate Koenig Co-authored-by: Ian Chen --- include/sdf/Element.hh | 9 +++- include/sdf/Gui.hh | 5 ++ include/sdf/Scene.hh | 5 ++ include/sdf/Sky.hh | 5 ++ include/sdf/World.hh | 16 +++++- sdf/1.9/scene.sdf | 2 +- src/Actor.cc | 5 +- src/Actor_TEST.cc | 10 ++-- src/Collision.cc | 4 +- src/Element.cc | 8 +++ src/Element_TEST.cc | 8 ++- src/Geometry.cc | 16 +++--- src/Gui.cc | 13 +++++ src/Gui_TEST.cc | 16 ++++++ src/Link.cc | 10 ++-- src/Link_TEST.cc | 39 +++++++------- src/Model.cc | 6 +-- src/Model_TEST.cc | 15 +++--- src/ParticleEmitter.cc | 2 +- src/Scene.cc | 19 +++++++ src/Scene_TEST.cc | 30 +++++++++++ src/Sky.cc | 22 ++++++++ src/Sky_TEST.cc | 30 +++++++++++ src/Visual.cc | 4 +- src/World.cc | 86 +++++++++++++++++++++++++++++++ src/World_TEST.cc | 113 +++++++++++++++++++++++++++++++++++++++++ src/parser.cc | 6 +-- 27 files changed, 441 insertions(+), 63 deletions(-) diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh index 0a0d3fbb9..99cc471c8 100644 --- a/include/sdf/Element.hh +++ b/include/sdf/Element.hh @@ -93,7 +93,7 @@ namespace sdf public: ElementPtr GetParent() const; /// \brief Set the parent of this Element. - /// \param[in] _parent Paren for this element. + /// \param[in] _parent Parent for this element. public: void SetParent(const ElementPtr _parent); /// \brief Set the name of the Element. @@ -462,6 +462,13 @@ namespace sdf /// \param[in] _elem the element object to add. public: void InsertElement(ElementPtr _elem); + /// \brief Add an element object, and optionally set the given element's + /// parent to this object. + /// \param[in] _elem the element object to add. + /// \param[in] _setParentToSelf If true, set the _elem's parent to this + /// object and then insert. + public: void InsertElement(ElementPtr _elem, bool _setParentToSelf); + /// \brief Remove this element from its parent. public: void RemoveFromParent(); diff --git a/include/sdf/Gui.hh b/include/sdf/Gui.hh index eec65d134..37e8391e9 100644 --- a/include/sdf/Gui.hh +++ b/include/sdf/Gui.hh @@ -61,6 +61,11 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Create and return an SDF element filled with data from this + /// gui. + /// \return SDF element pointer with updated gui values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Scene.hh b/include/sdf/Scene.hh index 2a137760c..c4a09106e 100644 --- a/include/sdf/Scene.hh +++ b/include/sdf/Scene.hh @@ -97,6 +97,11 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Create and return an SDF element filled with data from this + /// scene. + /// \return SDF element pointer with updated scene values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Sky.hh b/include/sdf/Sky.hh index cd76cfd47..e84693618 100644 --- a/include/sdf/Sky.hh +++ b/include/sdf/Sky.hh @@ -113,6 +113,11 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Create and return an SDF element filled with data from this + /// sky. + /// \return SDF element pointer with updated sky values. + public: sdf::ElementPtr ToElement() const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 4280cc57b..33cb0a01f 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -194,10 +194,16 @@ namespace sdf /// \brief Add a light to the world. /// \param[in] _light Light to add. - /// \return True if successful, false if a lights with the name already + /// \return True if successful, false if a light with the name already /// exists. public: bool AddLight(const Light &_light); + /// \brief Add a physics object to the world. + /// \param[in] _physics Physics to add. + /// \return True if successful, false if a physics object with the name + /// already exists. + public: bool AddPhysics(const Physics &_physics); + /// \brief Remove all models. public: void ClearModels(); @@ -207,6 +213,9 @@ namespace sdf /// \brief Remove all models. public: void ClearLights(); + /// \brief Remove all physics. + public: void ClearPhysics(); + /// \brief Get the number of actors. /// \return Number of actors contained in this World object. public: uint64_t ActorCount() const; @@ -352,6 +361,11 @@ namespace sdf public: const NestedInclude* InterfaceModelNestedIncludeByIndex( const uint64_t _index) const; + /// \brief Create and return an SDF element filled with data from this + /// world. + /// \return SDF element pointer with updated world values. + public: sdf::ElementPtr ToElement() const; + /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child /// entities for resolving poses. This is private and is intended to be /// called by Root::Load. diff --git a/sdf/1.9/scene.sdf b/sdf/1.9/scene.sdf index 0cdb52eb1..01b26a7a2 100644 --- a/sdf/1.9/scene.sdf +++ b/sdf/1.9/scene.sdf @@ -23,7 +23,7 @@ - Sunset time [0..24] + Information about clouds in the sky. Speed of the clouds diff --git a/src/Actor.cc b/src/Actor.cc index aaf43adb3..35872bf56 100644 --- a/src/Actor.cc +++ b/src/Actor.cc @@ -774,12 +774,11 @@ sdf::ElementPtr Actor::ToElement() const // Joints for (const sdf::Joint &joint : this->dataPtr->joints) - elem->InsertElement(joint.ToElement()); + elem->InsertElement(joint.ToElement(), true); // Link for (const sdf::Link &link : this->dataPtr->links) - elem->InsertElement(link.ToElement()); + elem->InsertElement(link.ToElement(), true); return elem; } - diff --git a/src/Actor_TEST.cc b/src/Actor_TEST.cc index 6b9ed9c03..1e676ad0e 100644 --- a/src/Actor_TEST.cc +++ b/src/Actor_TEST.cc @@ -516,7 +516,7 @@ TEST(DOMActor, ToElement) actor.SetScriptDelayStart(2.4); actor.SetScriptAutoStart(true); - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 1; ++i) { @@ -525,10 +525,11 @@ TEST(DOMActor, ToElement) EXPECT_TRUE(actor.AddLink(link)); EXPECT_FALSE(actor.AddLink(link)); } - actor.ClearLinks(); + if (j == 0) + actor.ClearLinks(); } - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 2; ++i) { @@ -537,7 +538,8 @@ TEST(DOMActor, ToElement) EXPECT_TRUE(actor.AddJoint(joint)); EXPECT_FALSE(actor.AddJoint(joint)); } - actor.ClearJoints(); + if (j == 0) + actor.ClearJoints(); } sdf::Trajectory trajectory; diff --git a/src/Collision.cc b/src/Collision.cc index cbeabaf9b..7a5929a50 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -217,10 +217,10 @@ sdf::ElementPtr Collision::ToElement() const poseElem->Set(this->RawPose()); // Set the geometry - elem->InsertElement(this->dataPtr->geom.ToElement()); + elem->InsertElement(this->dataPtr->geom.ToElement(), true); // Set the surface - elem->InsertElement(this->dataPtr->surface.ToElement()); + elem->InsertElement(this->dataPtr->surface.ToElement(), true); return elem; } diff --git a/src/Element.cc b/src/Element.cc index d9e710868..168403b8c 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -929,6 +929,14 @@ void Element::InsertElement(ElementPtr _elem) this->dataPtr->elements.push_back(_elem); } +///////////////////////////////////////////////// +void Element::InsertElement(ElementPtr _elem, bool _setParentToSelf) +{ + if (_setParentToSelf) + _elem->SetParent(shared_from_this()); + this->dataPtr->elements.push_back(_elem); +} + ///////////////////////////////////////////////// bool Element::HasElementDescription(const std::string &_name) const { diff --git a/src/Element_TEST.cc b/src/Element_TEST.cc index de58a067f..4261d12f9 100644 --- a/src/Element_TEST.cc +++ b/src/Element_TEST.cc @@ -91,11 +91,9 @@ TEST(Element, SetExplicitlySetInFile) sdf::ElementPtr parent = std::make_shared(); sdf::ElementPtr elem = std::make_shared(); - elem->SetParent(parent); - parent->InsertElement(elem); + parent->InsertElement(elem, true); sdf::ElementPtr elem2 = std::make_shared(); - elem2->SetParent(parent); - parent->InsertElement(elem2); + parent->InsertElement(elem2, true); EXPECT_TRUE(elem->GetExplicitlySetInFile()); @@ -111,7 +109,7 @@ TEST(Element, SetExplicitlySetInFile) // set to the same value when using this function sdf::ElementPtr child = std::make_shared(); child->SetParent(elem); - elem->InsertElement(child); + elem->InsertElement(child, false); sdf::ElementPtr sibling = std::make_shared(); sibling->SetParent(elem); diff --git a/src/Geometry.cc b/src/Geometry.cc index d2c933356..6fb2edb3d 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -281,28 +281,28 @@ sdf::ElementPtr Geometry::ToElement() const switch (this->dataPtr->type) { case GeometryType::BOX: - elem->InsertElement(this->dataPtr->box->ToElement()); + elem->InsertElement(this->dataPtr->box->ToElement(), true); break; case GeometryType::CYLINDER: - elem->InsertElement(this->dataPtr->cylinder->ToElement()); + elem->InsertElement(this->dataPtr->cylinder->ToElement(), true); break; case GeometryType::PLANE: - elem->InsertElement(this->dataPtr->plane->ToElement()); + elem->InsertElement(this->dataPtr->plane->ToElement(), true); break; case GeometryType::SPHERE: - elem->InsertElement(this->dataPtr->sphere->ToElement()); + elem->InsertElement(this->dataPtr->sphere->ToElement(), true); break; case GeometryType::MESH: - elem->InsertElement(this->dataPtr->mesh->ToElement()); + elem->InsertElement(this->dataPtr->mesh->ToElement(), true); break; case GeometryType::HEIGHTMAP: - elem->InsertElement(this->dataPtr->heightmap->ToElement()); + elem->InsertElement(this->dataPtr->heightmap->ToElement(), true); break; case GeometryType::CAPSULE: - elem->InsertElement(this->dataPtr->capsule->ToElement()); + elem->InsertElement(this->dataPtr->capsule->ToElement(), true); break; case GeometryType::ELLIPSOID: - elem->InsertElement(this->dataPtr->ellipsoid->ToElement()); + elem->InsertElement(this->dataPtr->ellipsoid->ToElement(), true); break; case GeometryType::EMPTY: default: diff --git a/src/Gui.cc b/src/Gui.cc index f78973c22..06620426b 100644 --- a/src/Gui.cc +++ b/src/Gui.cc @@ -15,6 +15,7 @@ * */ #include "sdf/Gui.hh" +#include "sdf/parser.hh" #include "Utils.hh" using namespace sdf; @@ -56,6 +57,8 @@ Errors Gui::Load(ElementPtr _sdf) this->dataPtr->fullscreen = _sdf->Get("fullscreen", this->dataPtr->fullscreen).first; + // \todo(nkoenig) Parse all the elements in gui.sdf + return errors; } @@ -82,3 +85,13 @@ sdf::ElementPtr Gui::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +sdf::ElementPtr Gui::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("gui.sdf", elem); + + elem->GetAttribute("fullscreen")->Set(this->dataPtr->fullscreen); + return elem; +} diff --git a/src/Gui_TEST.cc b/src/Gui_TEST.cc index a2ce6b844..aaef5ab26 100644 --- a/src/Gui_TEST.cc +++ b/src/Gui_TEST.cc @@ -123,3 +123,19 @@ TEST(DOMGui, Equal) gui.SetFullscreen(false); EXPECT_FALSE(gui == gui2); } + +///////////////////////////////////////////////// +TEST(DOMGui, ToElement) +{ + sdf::Gui gui; + + gui.SetFullscreen(true); + + sdf::ElementPtr elem = gui.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Gui gui2; + gui2.Load(elem); + + EXPECT_EQ(gui.Fullscreen(), gui2.Fullscreen()); +} diff --git a/src/Link.cc b/src/Link.cc index 14ff6fe10..0c51881c1 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -612,31 +612,31 @@ sdf::ElementPtr Link::ToElement() const // Collisions for (const sdf::Collision &collision : this->dataPtr->collisions) { - elem->InsertElement(collision.ToElement()); + elem->InsertElement(collision.ToElement(), true); } // Light for (const sdf::Light &light : this->dataPtr->lights) { - elem->InsertElement(light.ToElement()); + elem->InsertElement(light.ToElement(), true); } // Particle emitters for (const sdf::ParticleEmitter &emitter : this->dataPtr->emitters) { - elem->InsertElement(emitter.ToElement()); + elem->InsertElement(emitter.ToElement(), true); } // Sensors for (const sdf::Sensor &sensor : this->dataPtr->sensors) { - elem->InsertElement(sensor.ToElement()); + elem->InsertElement(sensor.ToElement(), true); } // Visuals for (const sdf::Visual &visual : this->dataPtr->visuals) { - elem->InsertElement(visual.ToElement()); + elem->InsertElement(visual.ToElement(), true); } return elem; diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index dc80a479c..8ba1e4ce5 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -319,7 +319,7 @@ TEST(DOMLink, ToElement) link.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); link.SetEnableWind(true); - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 1; ++i) { @@ -328,22 +328,24 @@ TEST(DOMLink, ToElement) EXPECT_TRUE(link.AddCollision(collision)); EXPECT_FALSE(link.AddCollision(collision)); } - link.ClearCollisions(); + if (j == 0) + link.ClearCollisions(); } - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { - for (int i = 0; i < 2; i++) - { - sdf::Visual visual; - visual.SetName("visual" + std::to_string(i)); - EXPECT_TRUE(link.AddVisual(visual)); - EXPECT_FALSE(link.AddVisual(visual)); - } - link.ClearVisuals(); + for (int i = 0; i < 2; i++) + { + sdf::Visual visual; + visual.SetName("visual" + std::to_string(i)); + EXPECT_TRUE(link.AddVisual(visual)); + EXPECT_FALSE(link.AddVisual(visual)); + } + if (j == 0) + link.ClearVisuals(); } - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 3; i++) { @@ -352,10 +354,11 @@ TEST(DOMLink, ToElement) EXPECT_TRUE(link.AddLight(light)); EXPECT_FALSE(link.AddLight(light)); } - link.ClearLights(); + if (j == 0) + link.ClearLights(); } - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 4; i++) { @@ -364,10 +367,11 @@ TEST(DOMLink, ToElement) EXPECT_TRUE(link.AddSensor(sensor)); EXPECT_FALSE(link.AddSensor(sensor)); } - link.ClearSensors(); + if (j == 0) + link.ClearSensors(); } - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 5; i++) { @@ -376,7 +380,8 @@ TEST(DOMLink, ToElement) EXPECT_TRUE(link.AddParticleEmitter(emitter)); EXPECT_FALSE(link.AddParticleEmitter(emitter)); } - link.ClearParticleEmitters(); + if (j == 0) + link.ClearParticleEmitters(); } sdf::ElementPtr elem = link.ToElement(); diff --git a/src/Model.cc b/src/Model.cc index bf5810803..b5cdbbcca 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -826,15 +826,15 @@ sdf::ElementPtr Model::ToElement() const // Links for (const sdf::Link &link : this->dataPtr->links) - elem->InsertElement(link.ToElement()); + elem->InsertElement(link.ToElement(), true); // Joints for (const sdf::Joint &joint : this->dataPtr->joints) - elem->InsertElement(joint.ToElement()); + elem->InsertElement(joint.ToElement(), true); // Model for (const sdf::Model &model : this->dataPtr->models) - elem->InsertElement(model.ToElement()); + elem->InsertElement(model.ToElement(), true); return elem; } diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 537ebe0ba..e6ab9d5d5 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -295,7 +295,7 @@ TEST(DOMModel, ToElement) model.SetEnableWind(true); model.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 1; ++i) { @@ -304,12 +304,13 @@ TEST(DOMModel, ToElement) EXPECT_TRUE(model.AddLink(link)); EXPECT_FALSE(model.AddLink(link)); } - model.ClearLinks(); + if (j == 0) + model.ClearLinks(); } model.SetCanonicalLinkName("link1"); model.SetPlacementFrameName("link0"); - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 2; ++i) { @@ -318,10 +319,11 @@ TEST(DOMModel, ToElement) EXPECT_TRUE(model.AddJoint(joint)); EXPECT_FALSE(model.AddJoint(joint)); } - model.ClearJoints(); + if (j == 0) + model.ClearJoints(); } - for (int j = 0; j < 1; ++j) + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 3; ++i) { @@ -330,7 +332,8 @@ TEST(DOMModel, ToElement) EXPECT_TRUE(model.AddModel(nestedModel)); EXPECT_FALSE(model.AddModel(nestedModel)); } - model.ClearModels(); + if (j == 0) + model.ClearModels(); } sdf::ElementPtr elem = model.ToElement(); diff --git a/src/ParticleEmitter.cc b/src/ParticleEmitter.cc index 7bb6a83a9..28c5414af 100644 --- a/src/ParticleEmitter.cc +++ b/src/ParticleEmitter.cc @@ -551,7 +551,7 @@ sdf::ElementPtr ParticleEmitter::ToElement() const if (this->dataPtr->material) { - elem->InsertElement(this->dataPtr->material->ToElement()); + elem->InsertElement(this->dataPtr->material->ToElement(), true); } return elem; diff --git a/src/Scene.cc b/src/Scene.cc index f99430d47..9825d601d 100644 --- a/src/Scene.cc +++ b/src/Scene.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include "sdf/parser.hh" #include "sdf/Scene.hh" #include "Utils.hh" @@ -176,3 +177,21 @@ sdf::ElementPtr Scene::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +sdf::ElementPtr Scene::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("scene.sdf", elem); + + elem->GetElement("ambient")->Set(this->Ambient()); + elem->GetElement("background")->Set(this->Background()); + elem->GetElement("grid")->Set(this->Grid()); + elem->GetElement("origin_visual")->Set(this->OriginVisual()); + elem->GetElement("shadows")->Set(this->Shadows()); + + if (this->dataPtr->sky) + elem->InsertElement(this->dataPtr->sky->ToElement(), true); + + return elem; +} diff --git a/src/Scene_TEST.cc b/src/Scene_TEST.cc index 0e10ece0d..97d3479ad 100644 --- a/src/Scene_TEST.cc +++ b/src/Scene_TEST.cc @@ -170,3 +170,33 @@ TEST(DOMScene, Set) scene.SetSky(sky); EXPECT_NE(nullptr, scene.Sky()); } + +///////////////////////////////////////////////// +TEST(DOMScene, ToElement) +{ + sdf::Scene scene; + + scene.SetAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f, 1.0f)); + scene.SetBackground(ignition::math::Color(0.2f, 0.3f, 0.4f, 1.0f)); + scene.SetGrid(true); + scene.SetOriginVisual(true); + scene.SetShadows(true); + + sdf::Sky sky; + scene.SetSky(sky); + + sdf::ElementPtr elem = scene.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Scene scene2; + scene2.Load(elem); + + EXPECT_EQ(scene.Ambient(), scene2.Ambient()); + EXPECT_EQ(scene.Background(), scene2.Background()); + EXPECT_EQ(scene.Grid(), scene2.Grid()); + EXPECT_EQ(scene.OriginVisual(), scene2.OriginVisual()); + EXPECT_EQ(scene.Shadows(), scene2.Shadows()); + + const sdf::Sky *sky2 = scene2.Sky(); + ASSERT_NE(nullptr, sky2); +} diff --git a/src/Sky.cc b/src/Sky.cc index 2635ac1f8..21651b07f 100644 --- a/src/Sky.cc +++ b/src/Sky.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include "sdf/parser.hh" #include "sdf/Sky.hh" #include "Utils.hh" @@ -201,3 +202,24 @@ sdf::ElementPtr Sky::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +sdf::ElementPtr Sky::ToElement() const +{ + sdf::ElementPtr sceneElem(new sdf::Element); + sdf::initFile("scene.sdf", sceneElem); + sdf::ElementPtr elem = sceneElem->GetElement("sky"); + + elem->GetElement("time")->Set(this->Time()); + elem->GetElement("sunrise")->Set(this->Sunrise()); + elem->GetElement("sunset")->Set(this->Sunset()); + + sdf::ElementPtr cloudElem = elem->GetElement("clouds"); + cloudElem->GetElement("speed")->Set(this->CloudSpeed()); + cloudElem->GetElement("direction")->Set(this->CloudDirection().Radian()); + cloudElem->GetElement("humidity")->Set(this->CloudHumidity()); + cloudElem->GetElement("mean_size")->Set(this->CloudMeanSize()); + cloudElem->GetElement("ambient")->Set(this->CloudAmbient()); + + return elem; +} diff --git a/src/Sky_TEST.cc b/src/Sky_TEST.cc index 03461bc43..e629493e5 100644 --- a/src/Sky_TEST.cc +++ b/src/Sky_TEST.cc @@ -186,3 +186,33 @@ TEST(DOMSky, Set) EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f), sky.CloudAmbient()); } + +///////////////////////////////////////////////// +TEST(DOMSky, ToElement) +{ + sdf::Sky sky; + + sky.SetTime(1.2); + sky.SetSunrise(0.5); + sky.SetSunset(10.2); + sky.SetCloudSpeed(100.2); + sky.SetCloudDirection(1.56); + sky.SetCloudHumidity(0.2); + sky.SetCloudMeanSize(0.5); + sky.SetCloudAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f, 1.0f)); + + sdf::ElementPtr elem = sky.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Sky sky2; + sky2.Load(elem); + + EXPECT_DOUBLE_EQ(sky.Time(), sky2.Time()); + EXPECT_DOUBLE_EQ(sky.Sunrise(), sky2.Sunrise()); + EXPECT_DOUBLE_EQ(sky.Sunset(), sky2.Sunset()); + EXPECT_DOUBLE_EQ(sky.CloudSpeed(), sky2.CloudSpeed()); + EXPECT_EQ(sky.CloudDirection(), sky2.CloudDirection()); + EXPECT_DOUBLE_EQ(sky.CloudHumidity(), sky2.CloudHumidity()); + EXPECT_DOUBLE_EQ(sky.CloudMeanSize(), sky2.CloudMeanSize()); + EXPECT_EQ(sky.CloudAmbient(), sky2.CloudAmbient()); +} diff --git a/src/Visual.cc b/src/Visual.cc index 4421f66c8..f49118c2e 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -320,7 +320,7 @@ sdf::ElementPtr Visual::ToElement() const poseElem->Set(this->RawPose()); // Set the geometry - elem->InsertElement(this->dataPtr->geom.ToElement()); + elem->InsertElement(this->dataPtr->geom.ToElement(), true); elem->GetElement("cast_shadows")->Set(this->CastShadows()); elem->GetElement("laser_retro")->Set(this->LaserRetro()); @@ -329,7 +329,7 @@ sdf::ElementPtr Visual::ToElement() const if (this->dataPtr->material) { - elem->InsertElement(this->dataPtr->material->ToElement()); + elem->InsertElement(this->dataPtr->material->ToElement(), true); } return elem; diff --git a/src/World.cc b/src/World.cc index 64ed5ddb4..59041eec1 100644 --- a/src/World.cc +++ b/src/World.cc @@ -798,6 +798,72 @@ Errors World::Implementation::LoadSphericalCoordinates( return errors; } +///////////////////////////////////////////////// +sdf::ElementPtr World::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("world.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + elem->GetElement("gravity")->Set(this->Gravity()); + elem->GetElement("magnetic_field")->Set(this->MagneticField()); + + sdf::ElementPtr windElem = elem->GetElement("wind"); + windElem->GetElement("linear_velocity")->Set(this->WindLinearVelocity()); + + // Physics + for (const sdf::Physics &physics : this->dataPtr->physics) + elem->InsertElement(physics.ToElement(), true); + + // Models + for (const sdf::Model &model : this->dataPtr->models) + elem->InsertElement(model.ToElement(), true); + + // Actors + for (const sdf::Actor &actor : this->dataPtr->actors) + elem->InsertElement(actor.ToElement(), true); + + // Lights + for (const sdf::Light &light : this->dataPtr->lights) + elem->InsertElement(light.ToElement(), true); + + // Spherical coordinates. + if (this->dataPtr->sphericalCoordinates) + { + sdf::ElementPtr sphericalElem = elem->GetElement("spherical_coordinates"); + sphericalElem->GetElement("surface_model")->Set( + ignition::math::SphericalCoordinates::Convert( + this->dataPtr->sphericalCoordinates->Surface())); + sphericalElem->GetElement("world_frame_orientation")->Set("ENU"); + sphericalElem->GetElement("latitude_deg")->Set( + this->dataPtr->sphericalCoordinates->LatitudeReference().Degree()); + sphericalElem->GetElement("longitude_deg")->Set( + this->dataPtr->sphericalCoordinates->LongitudeReference().Degree()); + sphericalElem->GetElement("elevation")->Set( + this->dataPtr->sphericalCoordinates->ElevationReference()); + sphericalElem->GetElement("heading_deg")->Set( + this->dataPtr->sphericalCoordinates->HeadingOffset().Degree()); + } + + // Atmosphere + if (this->dataPtr->atmosphere) + elem->InsertElement(this->dataPtr->atmosphere->ToElement(), true); + + // Gui + if (this->dataPtr->gui) + elem->InsertElement(this->dataPtr->gui->ToElement(), true); + + // Scene + if (this->dataPtr->scene) + elem->InsertElement(this->dataPtr->scene->ToElement(), true); + + // Audio + if (this->dataPtr->audioDevice != "default") + elem->GetElement("audio")->GetElement("device")->Set(this->AudioDevice()); + + return elem; +} + ///////////////////////////////////////////////// void World::ClearModels() { @@ -816,6 +882,12 @@ void World::ClearLights() this->dataPtr->lights.clear(); } +///////////////////////////////////////////////// +void World::ClearPhysics() +{ + this->dataPtr->physics.clear(); +} + ///////////////////////////////////////////////// bool World::AddModel(const Model &_model) { @@ -844,3 +916,17 @@ bool World::AddLight(const Light &_light) return true; } + +///////////////////////////////////////////////// +bool World::AddPhysics(const Physics &_physics) +{ + if (this->PhysicsNameExists(_physics.Name())) + { + std::cout << "Not adding physics, it exists\n"; + return false; + } + std::cout << "Adding physics\n"; + this->dataPtr->physics.push_back(_physics); + + return true; +} diff --git a/src/World_TEST.cc b/src/World_TEST.cc index 7a76f33c8..74dd57e1b 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -21,6 +21,7 @@ #include "sdf/Light.hh" #include "sdf/Actor.hh" #include "sdf/Model.hh" +#include "sdf/Physics.hh" #include "sdf/World.hh" ///////////////////////////////////////////////// @@ -420,3 +421,115 @@ TEST(DOMWorld, AddLight) ASSERT_NE(nullptr, lightFromWorld); EXPECT_EQ(lightFromWorld->Name(), light.Name()); } + +///////////////////////////////////////////////// +TEST(DOMWorld, ToElement) +{ + sdf::World world; + + world.SetName("my-world"); + world.SetAudioDevice("my-audio"); + world.SetWindLinearVelocity(ignition::math::Vector3d(1, 2, 3)); + world.SetGravity(ignition::math::Vector3d(-1, 5, 10)); + world.SetMagneticField(ignition::math::Vector3d(2.0, 0.1, 0.5)); + world.SetSphericalCoordinates(ignition::math::SphericalCoordinates()); + + sdf::Atmosphere atmosphere; + world.SetAtmosphere(atmosphere); + + sdf::Gui gui; + world.SetGui(gui); + + sdf::Scene scene; + world.SetScene(scene); + + for (int j = 0; j <= 1; ++j) + { + for (int i = 0; i < 2; ++i) + { + sdf::Model model; + model.SetName("model" + std::to_string(i)); + EXPECT_TRUE(world.AddModel(model)); + EXPECT_FALSE(world.AddModel(model)); + } + if (j == 0) + world.ClearModels(); + } + + for (int j = 0; j <= 1; ++j) + { + for (int i = 0; i < 3; ++i) + { + sdf::Actor actor; + actor.SetName("actor" + std::to_string(i)); + EXPECT_TRUE(world.AddActor(actor)); + EXPECT_FALSE(world.AddActor(actor)); + } + if (j == 0) + world.ClearActors(); + } + + for (int j = 0; j <= 1; ++j) + { + for (int i = 0; i < 4; ++i) + { + sdf::Light light; + light.SetName("light" + std::to_string(i)); + EXPECT_TRUE(world.AddLight(light)); + EXPECT_FALSE(world.AddLight(light)); + } + if (j == 0) + world.ClearLights(); + } + + for (int j = 0; j <= 1; ++j) + { + for (int i = 0; i < 5; ++i) + { + sdf::Physics physics; + physics.SetName("physics" + std::to_string(i)); + EXPECT_TRUE(world.AddPhysics(physics)); + EXPECT_FALSE(world.AddPhysics(physics)); + } + if (j == 0) + world.ClearPhysics(); + } + + sdf::ElementPtr elem = world.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::World world2; + world2.Load(elem); + + EXPECT_EQ(world.Name(), world2.Name()); + EXPECT_EQ(world.AudioDevice(), world2.AudioDevice()); + EXPECT_EQ(world.WindLinearVelocity(), world2.WindLinearVelocity()); + EXPECT_EQ(world.Gravity(), world2.Gravity()); + EXPECT_EQ(world.MagneticField(), world2.MagneticField()); + EXPECT_EQ(*world.SphericalCoordinates(), *world2.SphericalCoordinates()); + + const sdf::Atmosphere *atmosphere2 = world2.Atmosphere(); + ASSERT_NE(nullptr, atmosphere2); + + const sdf::Gui *gui2 = world2.Gui(); + ASSERT_NE(nullptr, gui2); + + const sdf::Scene *scene2 = world2.Scene(); + ASSERT_NE(nullptr, scene2); + + EXPECT_EQ(world.ModelCount(), world2.ModelCount()); + for (uint64_t i = 0; i < world2.ModelCount(); ++i) + EXPECT_NE(nullptr, world2.ModelByIndex(i)); + + EXPECT_EQ(world.LightCount(), world2.LightCount()); + for (uint64_t i = 0; i < world2.LightCount(); ++i) + EXPECT_NE(nullptr, world2.LightByIndex(i)); + + EXPECT_EQ(world.ActorCount(), world2.ActorCount()); + for (uint64_t i = 0; i < world2.ActorCount(); ++i) + EXPECT_NE(nullptr, world2.ActorByIndex(i)); + + EXPECT_EQ(world.PhysicsCount(), world2.PhysicsCount()); + for (uint64_t i = 0; i < world2.PhysicsCount(); ++i) + EXPECT_NE(nullptr, world2.PhysicsByIndex(i)); +} diff --git a/src/parser.cc b/src/parser.cc index a3cec154a..0c45332b0 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -203,8 +203,7 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF, if (!_merge) { - firstElem->SetParent(_parent); - _parent->InsertElement(firstElem); + _parent->InsertElement(firstElem, true); return; } else if (firstElem->GetName() != "model") @@ -347,8 +346,7 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF, (elem->GetName() == "gripper") || (elem->GetName() == "plugin") || (elem->GetName().find(':') != std::string::npos)) { - elem->SetParent(_parent); - _parent->InsertElement(elem); + _parent->InsertElement(elem, true); } } } From 4a6b5b683455e5c67649296031edeedc51854258 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 13 Dec 2021 16:03:43 -0800 Subject: [PATCH 55/60] Support adding and clearing sensors from a joint (#785) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/Joint.hh | 9 +++++++++ src/Joint.cc | 15 +++++++++++++++ src/Joint_TEST.cc | 18 ++++++++++++++++++ 3 files changed, 42 insertions(+) diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 55f855c61..fdd59502e 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -230,6 +230,15 @@ namespace sdf /// \return SDF element pointer with updated joint values. public: sdf::ElementPtr ToElement() const; + /// \brief Add a sensors to the joint. + /// \param[in] _sensor Sensor to add. + /// \return True if successful, false if a sensor with the name already + /// exists. + public: bool AddSensor(const Sensor &_sensor); + + /// \brief Remove all sensors. + public: void ClearSensors(); + /// \brief Give the scoped FrameAttachedToGraph to be used for resolving /// parent and child link names. This is private and is intended to be /// called by Model::Load. diff --git a/src/Joint.cc b/src/Joint.cc index 0e77e1f34..1a9e64e84 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -528,3 +528,18 @@ sdf::ElementPtr Joint::ToElement() const // supported. return elem; } + +////////////////////////////////////////////////// +bool Joint::AddSensor(const Sensor &_sensor) +{ + if (this->SensorNameExists(_sensor.Name())) + return false; + this->dataPtr->sensors.push_back(_sensor); + return true; +} + +////////////////////////////////////////////////// +void Joint::ClearSensors() +{ + this->dataPtr->sensors.clear(); +} diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index e00463c20..ee7b7e69c 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -18,6 +18,7 @@ #include #include #include "sdf/Joint.hh" +#include "sdf/Sensor.hh" #include "sdf/JointAxis.hh" ///////////////////////////////////////////////// @@ -269,6 +270,19 @@ TEST(DOMJoint, ToElement) EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); joint.SetAxis(1, axis1); + for (int j = 0; j <= 1; ++j) + { + for (int i = 0; i < 3; ++i) + { + sdf::Sensor sensor; + sensor.SetName("sensor" + std::to_string(i)); + EXPECT_TRUE(joint.AddSensor(sensor)); + EXPECT_FALSE(joint.AddSensor(sensor)); + } + if (j == 0) + joint.ClearSensors(); + } + sdf::ElementPtr jointElem = joint.ToElement(); EXPECT_NE(nullptr, jointElem); EXPECT_EQ(nullptr, joint.Element()); @@ -289,6 +303,10 @@ TEST(DOMJoint, ToElement) EXPECT_EQ(axis.Xyz(), joint2.Axis(0)->Xyz()); EXPECT_EQ(axis1.Xyz(), joint2.Axis(1)->Xyz()); + EXPECT_EQ(joint.SensorCount(), joint.SensorCount()); + for (uint64_t i = 0; i < joint.SensorCount(); ++i) + EXPECT_NE(nullptr, joint.SensorByIndex(i)); + // make changes to DOM and verify ToElement produces updated values joint2.SetParentLinkName("new_parent"); sdf::ElementPtr joint2Elem = joint2.ToElement(); From 1a889721e3ab3066e06ec4d15056f4ca1d2082aa Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 17 Dec 2021 10:42:15 -0800 Subject: [PATCH 56/60] Support URI in the Model DOM (#786) * Support URI in the Model DOM Signed-off-by: Nate Koenig * One minor test Signed-off-by: Nate Koenig * placement frame and static Signed-off-by: Nate Koenig * relative_to Signed-off-by: Nate Koenig * Added nested include test Signed-off-by: Nate Koenig * Updates Signed-off-by: Nate Koenig * useincludetag Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/Model.hh | 18 ++++++- src/Model.cc | 41 +++++++++++++-- src/Model_TEST.cc | 115 +++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 170 insertions(+), 4 deletions(-) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 75ed0420d..ec8b5684f 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -348,8 +348,16 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// model. + /// \param[in] _useIncludeTag When true, the model's URI is used to create + /// an SDF `` rather than a ``. The model's URI must be + /// first set using the `Model::SetUri` function. If the model's URI is + /// empty, then a `` element will be generated. The default is true + /// so that URI values are used when ToElement is called from a + /// World object. Make sure to use `Model::SetUri` even when the model + /// is loaded from an `` tag since the parser will + /// automatically expand an `` element to a `` element. /// \return SDF element pointer with updated model values. - public: sdf::ElementPtr ToElement() const; + public: sdf::ElementPtr ToElement(bool _useIncludeTag = true) const; /// \brief Add a link to the model. /// \param[in] _link Link to add. @@ -378,6 +386,14 @@ namespace sdf /// \brief Remove all models. public: void ClearModels(); + /// \brief Get the URI associated with this model + /// \return The model's URI, or empty string if it has not been set. + public: std::string Uri() const; + + /// \brief Set the URI associated with this model. + /// \param[in] _uri The model's URI. + public: void SetUri(const std::string &_uri); + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Root::Load or /// World::SetPoseRelativeToGraph if this is a standalone model and diff --git a/src/Model.cc b/src/Model.cc index b5cdbbcca..eedf320e4 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -94,6 +94,10 @@ class sdf::Model::Implementation /// \brief Scope name of parent Pose Relative-To Graph (world or __model__). public: std::string poseGraphScopeVertexName; + + /// \brief Optional URI string that specifies where this model was or + /// can be loaded from. + public: std::string uri = ""; }; ///////////////////////////////////////////////// @@ -792,11 +796,42 @@ const NestedInclude *Model::InterfaceModelNestedIncludeByIndex( } ///////////////////////////////////////////////// -sdf::ElementPtr Model::ToElement() const +std::string Model::Uri() const +{ + return this->dataPtr->uri; +} + +///////////////////////////////////////////////// +void Model::SetUri(const std::string &_uri) +{ + this->dataPtr->uri = _uri; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Model::ToElement(bool _useIncludeTag) const { + if (_useIncludeTag && !this->dataPtr->uri.empty()) + { + sdf::ElementPtr worldElem(new sdf::Element); + sdf::initFile("world.sdf", worldElem); + + sdf::ElementPtr includeElem = worldElem->AddElement("include"); + includeElem->GetElement("uri")->Set(this->Uri()); + includeElem->GetElement("name")->Set(this->Name()); + includeElem->GetElement("pose")->Set(this->RawPose()); + if (!this->dataPtr->poseRelativeTo.empty()) + { + includeElem->GetElement("pose")->GetAttribute( + "relative_to")->Set(this->dataPtr->poseRelativeTo); + } + includeElem->GetElement("static")->Set(this->Static()); + includeElem->GetElement("placement_frame")->Set(this->PlacementFrameName()); + + return includeElem; + } + sdf::ElementPtr elem(new sdf::Element); sdf::initFile("model.sdf", elem); - elem->GetAttribute("name")->Set(this->Name()); if (!this->dataPtr->canonicalLink.empty()) @@ -834,7 +869,7 @@ sdf::ElementPtr Model::ToElement() const // Model for (const sdf::Model &model : this->dataPtr->models) - elem->InsertElement(model.ToElement(), true); + elem->InsertElement(model.ToElement(_useIncludeTag), true); return elem; } diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index e6ab9d5d5..f7406d01f 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -20,6 +20,8 @@ #include "sdf/Joint.hh" #include "sdf/Link.hh" #include "sdf/Model.hh" +#include "sdf/parser.hh" +#include "test_config.h" ///////////////////////////////////////////////// /// Test default construction of sdf::Model. @@ -363,3 +365,116 @@ TEST(DOMModel, ToElement) for (uint64_t i = 0; i < model2.ModelCount(); ++i) EXPECT_NE(nullptr, model2.ModelByIndex(i)); } + +///////////////////////////////////////////////// +TEST(DOMModel, Uri) +{ + sdf::Model model; + std::string name = "my-model"; + ignition::math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + std::string uri = + "https://fuel.ignitionrobotics.org/1.0/openrobotics/models/my-model"; + + model.SetName(name); + model.SetRawPose(pose); + model.SetStatic(true); + model.SetPlacementFrameName("link0"); + model.SetPoseRelativeTo("other"); + model.SetUri(uri); + EXPECT_EQ(uri, model.Uri()); + + // ToElement using the URI, which should result in an + { + sdf::ElementPtr elem = model.ToElement(); + EXPECT_EQ("include", elem->GetName()); + + sdf::ElementPtr uriElem = elem->FindElement("uri"); + ASSERT_NE(nullptr, uriElem); + EXPECT_EQ(uri, uriElem->Get()); + + sdf::ElementPtr nameElem = elem->FindElement("name"); + ASSERT_NE(nullptr, nameElem); + EXPECT_EQ(name, nameElem->Get()); + + sdf::ElementPtr poseElem = elem->FindElement("pose"); + ASSERT_NE(nullptr, poseElem); + EXPECT_EQ(pose, poseElem->Get()); + EXPECT_EQ("other", poseElem->GetAttribute("relative_to")->GetAsString()); + + EXPECT_EQ("link0", + elem->FindElement("placement_frame")->Get()); + + sdf::ElementPtr staticElem = elem->FindElement("static"); + ASSERT_NE(nullptr, staticElem); + EXPECT_EQ(true, staticElem->Get()); + } + + // ToElement NOT using the URI, which should result in a + { + sdf::ElementPtr elem = model.ToElement(false); + elem->PrintValues(" "); + + // Should be a + EXPECT_EQ("model", elem->GetName()); + + // URI should not exist + sdf::ElementPtr uriElem = elem->FindElement("uri"); + ASSERT_EQ(nullptr, uriElem); + + sdf::ParamPtr nameAttr = elem->GetAttribute("name"); + ASSERT_NE(nullptr, nameAttr); + EXPECT_EQ(name, nameAttr->GetAsString()); + + sdf::ParamPtr placementFrameAttr = elem->GetAttribute("placement_frame"); + ASSERT_NE(nullptr, placementFrameAttr); + EXPECT_EQ("link0", placementFrameAttr->GetAsString()); + + sdf::ElementPtr poseElem = elem->FindElement("pose"); + ASSERT_NE(nullptr, poseElem); + EXPECT_EQ(pose, poseElem->Get()); + EXPECT_EQ("other", poseElem->GetAttribute("relative_to")->GetAsString()); + + sdf::ElementPtr staticElem = elem->FindElement("static"); + ASSERT_NE(nullptr, staticElem); + EXPECT_EQ(true, staticElem->Get()); + } +} + +///////////////////////////////////////////////// +TEST(DOMModel, ToElementNestedHasUri) +{ + sdf::Model model; + model.SetName("parent"); + EXPECT_EQ(0u, model.ModelCount()); + + sdf::Model nestedModel; + nestedModel.SetName("child"); + nestedModel.SetUri("child-uri"); + EXPECT_TRUE(model.AddModel(nestedModel)); + EXPECT_EQ(1u, model.ModelCount()); + + sdf::Model nestedModel2; + nestedModel2.SetName("child2"); + EXPECT_TRUE(model.AddModel(nestedModel2)); + EXPECT_EQ(2u, model.ModelCount()); + + sdf::ElementPtr elem = model.ToElement(); + + // The parent model does not have a URI, so the element name should be + // "model". + ASSERT_NE(nullptr, elem); + EXPECT_EQ("model", elem->GetName()); + + // Get the child element, which should exist because the nested + // model has a URI. + sdf::ElementPtr includeElem = elem->FindElement("include"); + ASSERT_NE(nullptr, includeElem); + ASSERT_NE(nullptr, includeElem->FindElement("uri")); + EXPECT_EQ("child-uri", includeElem->FindElement("uri")->Get()); + + // Get the child element, which should exist because one nested + // model does not have a URI. + sdf::ElementPtr modelElem = elem->FindElement("model"); + ASSERT_NE(nullptr, modelElem); + EXPECT_EQ("child2", modelElem->GetAttribute("name")->GetAsString()); +} From ef48530765297126ff3c3486a926f4ae650c1331 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 17 Dec 2021 15:09:02 -0800 Subject: [PATCH 57/60] Added plugin to SDF DOM (#788) * Added plugin to SDF DOM Signed-off-by: Nate Koenig * tweaks Signed-off-by: Nate Koenig * Fixed doxygen Signed-off-by: Nate Koenig * Updates Signed-off-by: Nate Koenig * Update plugin copy and tests Signed-off-by: Nate Koenig * Remove string and add move functions Signed-off-by: Nate Koenig * Fix build and tests Signed-off-by: Nate Koenig * Fix windows warnings Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/sdf/CMakeLists.txt | 1 + include/sdf/Gui.hh | 19 +++ include/sdf/Plugin.hh | 135 +++++++++++++++++++ sdf/1.9/plugin.sdf | 2 +- src/CMakeLists.txt | 2 + src/Gui.cc | 38 ++++++ src/Gui_TEST.cc | 21 +++ src/Plugin.cc | 204 ++++++++++++++++++++++++++++ src/Plugin_TEST.cc | 263 +++++++++++++++++++++++++++++++++++++ 9 files changed, 684 insertions(+), 1 deletion(-) create mode 100644 include/sdf/Plugin.hh create mode 100644 src/Plugin.cc create mode 100644 src/Plugin_TEST.cc diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index e1422b316..686bf7c8a 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -47,6 +47,7 @@ set (headers Pbr.hh Physics.hh Plane.hh + Plugin.hh PrintConfig.hh Root.hh Scene.hh diff --git a/include/sdf/Gui.hh b/include/sdf/Gui.hh index 37e8391e9..35f9f0b6a 100644 --- a/include/sdf/Gui.hh +++ b/include/sdf/Gui.hh @@ -19,6 +19,7 @@ #include #include "sdf/Element.hh" +#include "sdf/Plugin.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" @@ -66,6 +67,24 @@ namespace sdf /// \return SDF element pointer with updated gui values. public: sdf::ElementPtr ToElement() const; + /// \brief Get the number of plugins. + /// \return Number of plugins contained in this Gui object. + public: uint64_t PluginCount() const; + + /// \brief Get a plugin based on an index. + /// \param[in] _index Index of the plugin. The index should be in the + /// range [0..PluginCount()). + /// \return Pointer to the plugin. Nullptr if the index does not exist. + /// \sa uint64_t PluginCount() const + public: const Plugin *PluginByIndex(const uint64_t _index) const; + + /// \brief Remove all plugins + public: void ClearPlugins(); + + /// \brief Add a plugin to the link. + /// \param[in] _plugin Plugin to add. + public: void AddPlugin(const Plugin &_plugin); + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Plugin.hh b/include/sdf/Plugin.hh new file mode 100644 index 000000000..2026b8ae9 --- /dev/null +++ b/include/sdf/Plugin.hh @@ -0,0 +1,135 @@ +/* + * Copyright 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef SDF_PLUGIN_HH_ +#define SDF_PLUGIN_HH_ + +#include +#include +#include + +#include +#include +#include +#include "sdf/sdf_config.h" +#include "sdf/system_util.hh" + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + class PluginPrivate; + + class SDFORMAT_VISIBLE Plugin + { + /// \brief Default constructor + public: Plugin(); + + /// \brief Default destructor + public: ~Plugin(); + + /// \brief Copy constructor. + /// \param[in] _plugin Plugin to copy. + public: Plugin(const Plugin &_plugin); + + /// \brief Move constructor. + /// \param[in] _plugin Plugin to copy. + public: Plugin(Plugin &&_plugin) noexcept; + + /// \brief Load the plugin 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); + + /// \brief Get the name of the plugin. + /// The name of the plugin should be unique within the scope of its + /// parent. + /// \return Name of the plugin. + public: std::string Name() const; + + /// \brief Set the name of the plugin. + /// The name of the plugin should be unique within the scope of its + /// parent. + /// \param[in] _name Name of the plugin. + public: void SetName(const std::string &_name); + + /// \brief Get the filename of the shared library. + /// \return Filename of the shared library associated with the plugin. + public: std::string Filename() const; + + /// \brief Remove the contents of the plugin, this is everything that + /// is a child element of the ``. + public: void ClearContents(); + + /// \brief Get the plugin contents. This is all the SDF elements that + /// are children of the ``. + /// \return The child elements of this plugin. + public: const std::vector &Contents() const; + + /// \brief Insert an element into the plugin content. This does not + /// modify the values in the sdf::ElementPtr returned by the `Element()` + /// function. + /// \param[in] _elem Element to insert. + public: void InsertContent(const sdf::ElementPtr _elem); + + /// \brief Set the filename of the shared library. + /// \param[in] _filename Filename of the shared library associated with + /// this plugin. + public: void SetFilename(const std::string &_filename); + + /// \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 + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Create and return an SDF element filled with data from this + /// plugin. + /// \return SDF element pointer with updated plugin values. + public: sdf::ElementPtr ToElement() const; + + /// \brief Copy assignment operator + /// \param[in] _plugin Plugin to copy + /// \return A reference to this plugin + public: Plugin &operator=(const Plugin &_plugin); + + /// \brief Move assignment operator + /// \param[in] _plugin Plugin to move + /// \return A reference to this plugin + public: Plugin &operator=(Plugin &&_plugin) noexcept; + + /// \brief Private data pointer. + std::unique_ptr dataPtr; + }; +} +} + +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#endif diff --git a/sdf/1.9/plugin.sdf b/sdf/1.9/plugin.sdf index 236ea31af..81452b64c 100644 --- a/sdf/1.9/plugin.sdf +++ b/sdf/1.9/plugin.sdf @@ -2,7 +2,7 @@ A plugin is a dynamically loaded chunk of code. It can exist as a child of world, model, and sensor. - A unique name for the plugin, scoped to its parent. + A name for the plugin. Name of the shared library to load. If the filename is not a full path name, the file will be searched for in the configuration paths. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a36fe8eda..597c6f5d8 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -60,6 +60,7 @@ set (sources Pbr.cc Physics.cc Plane.cc + Plugin.cc PrintConfig.cc Root.cc Scene.cc @@ -134,6 +135,7 @@ if (BUILD_SDF_TEST) ParticleEmitter_TEST.cc Pbr_TEST.cc Physics_TEST.cc + Plugin_TEST.cc PrintConfig_TEST.cc Plane_TEST.cc Root_TEST.cc diff --git a/src/Gui.cc b/src/Gui.cc index 06620426b..dc0a9e3e0 100644 --- a/src/Gui.cc +++ b/src/Gui.cc @@ -28,6 +28,9 @@ class sdf::Gui::Implementation /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; + + /// \brief GUI plugins. + public: std::vector plugins; }; ///////////////////////////////////////////////// @@ -57,6 +60,10 @@ Errors Gui::Load(ElementPtr _sdf) this->dataPtr->fullscreen = _sdf->Get("fullscreen", this->dataPtr->fullscreen).first; + Errors pluginErrors = loadRepeated(_sdf, "plugin", + this->dataPtr->plugins); + errors.insert(errors.end(), pluginErrors.begin(), pluginErrors.end()); + // \todo(nkoenig) Parse all the elements in gui.sdf return errors; @@ -93,5 +100,36 @@ sdf::ElementPtr Gui::ToElement() const sdf::initFile("gui.sdf", elem); elem->GetAttribute("fullscreen")->Set(this->dataPtr->fullscreen); + + // Add in the plugins + for (const Plugin &plugin : this->dataPtr->plugins) + elem->InsertElement(plugin.ToElement(), true); + return elem; } + +///////////////////////////////////////////////// +uint64_t Gui::PluginCount() const +{ + return this->dataPtr->plugins.size(); +} + +///////////////////////////////////////////////// +const Plugin *Gui::PluginByIndex(const uint64_t _index) const +{ + if (_index < this->dataPtr->plugins.size()) + return &this->dataPtr->plugins[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +void Gui::ClearPlugins() +{ + this->dataPtr->plugins.clear(); +} + +///////////////////////////////////////////////// +void Gui::AddPlugin(const Plugin &_plugin) +{ + this->dataPtr->plugins.push_back(_plugin); +} diff --git a/src/Gui_TEST.cc b/src/Gui_TEST.cc index aaef5ab26..f068d5034 100644 --- a/src/Gui_TEST.cc +++ b/src/Gui_TEST.cc @@ -131,6 +131,26 @@ TEST(DOMGui, ToElement) gui.SetFullscreen(true); + for (int j = 0; j <= 1; ++j) + { + for (int i = 0; i < 3; ++i) + { + sdf::Plugin plugin; + plugin.SetName("name" + std::to_string(i)); + plugin.SetFilename("filename" + std::to_string(i)); + gui.AddPlugin(plugin); + gui.AddPlugin(plugin); + } + if (j == 0) + { + EXPECT_EQ(6u, gui.PluginCount()); + gui.ClearPlugins(); + EXPECT_EQ(0u, gui.PluginCount()); + } + } + + + EXPECT_EQ(6u, gui.PluginCount()); sdf::ElementPtr elem = gui.ToElement(); ASSERT_NE(nullptr, elem); @@ -138,4 +158,5 @@ TEST(DOMGui, ToElement) gui2.Load(elem); EXPECT_EQ(gui.Fullscreen(), gui2.Fullscreen()); + EXPECT_EQ(6u, gui2.PluginCount()); } diff --git a/src/Plugin.cc b/src/Plugin.cc new file mode 100644 index 000000000..d59e6bb64 --- /dev/null +++ b/src/Plugin.cc @@ -0,0 +1,204 @@ +/* + * Copyright 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "sdf/Plugin.hh" +#include "sdf/parser.hh" +#include "Utils.hh" + +using namespace sdf; + +class sdf::PluginPrivate +{ + /// \brief Name of the plugin + public: std::string name = ""; + + /// \brief Filename of the shared library + public: std::string filename = ""; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; + + /// \brief SDF elements inside the plugin. + public: std::vector contents; +}; + +///////////////////////////////////////////////// +Plugin::Plugin() + : dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +Plugin::~Plugin() = default; + +///////////////////////////////////////////////// +Plugin::Plugin(const Plugin &_plugin) + : dataPtr(std::make_unique()) +{ + // Copy + *this = _plugin; +} + +///////////////////////////////////////////////// +Plugin::Plugin(Plugin &&_plugin) noexcept +{ + this->dataPtr = std::move(_plugin.dataPtr); +} + +///////////////////////////////////////////////// +Errors Plugin::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a plugin, but the provided SDF " + "element is null."}); + return errors; + } + + // We need a plugin element + if (_sdf->GetName() != "plugin") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a plugin, but the provided SDF " + "element is not a ."}); + return errors; + } + + // Read the models's name + if (!loadName(_sdf, this->dataPtr->name)) + { + errors.push_back({ErrorCode::ATTRIBUTE_MISSING, + "A plugin name is required, but the name is not set."}); + } + + // Read the filename + std::pair filenamePair = + _sdf->Get("filename", this->dataPtr->filename); + this->dataPtr->filename = filenamePair.first; + if (!filenamePair.second) + { + errors.push_back({ErrorCode::ATTRIBUTE_MISSING, + "A plugin filename is required, but the filename is not set."}); + } + + // Copy the contents of the plugin + for (sdf::ElementPtr innerElem = _sdf->GetFirstElement(); + innerElem; innerElem = innerElem->GetNextElement("")) + { + this->dataPtr->contents.push_back(innerElem->Clone()); + } + + return errors; +} + +///////////////////////////////////////////////// +std::string Plugin::Name() const +{ + return this->dataPtr->name; +} + +///////////////////////////////////////////////// +void Plugin::SetName(const std::string &_name) +{ + this->dataPtr->name = _name; +} + +///////////////////////////////////////////////// +std::string Plugin::Filename() const +{ + return this->dataPtr->filename; +} + +///////////////////////////////////////////////// +void Plugin::SetFilename(const std::string &_filename) +{ + this->dataPtr->filename = _filename; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Plugin::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Plugin::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("plugin.sdf", elem); + + elem->GetAttribute("name")->Set(this->Name()); + elem->GetAttribute("filename")->Set(this->Filename()); + + // Insert plugin content + for (const sdf::ElementPtr content : this->dataPtr->contents) + elem->InsertElement(content, true); + + return elem; +} + +///////////////////////////////////////////////// +void Plugin::ClearContents() +{ + this->dataPtr->contents.clear(); +} + +///////////////////////////////////////////////// +const std::vector &Plugin::Contents() const +{ + return this->dataPtr->contents; +} + +///////////////////////////////////////////////// +void Plugin::InsertContent(const sdf::ElementPtr _elem) +{ + this->dataPtr->contents.push_back(_elem->Clone()); +} + +///////////////////////////////////////////////// +Plugin &Plugin::operator=(const Plugin &_plugin) +{ + if (!this->dataPtr) + this->dataPtr = std::make_unique(); + + this->dataPtr->name = _plugin.Name(); + this->dataPtr->filename = _plugin.Filename(); + if (_plugin.Element()) + this->dataPtr->sdf = _plugin.Element()->Clone(); + + this->dataPtr->contents.clear(); + // Copy the contents of the plugin + for (const sdf::ElementPtr content : _plugin.Contents()) + { + this->dataPtr->contents.push_back(content->Clone()); + } + + return *this; +} + +///////////////////////////////////////////////// +Plugin &Plugin::operator=(Plugin &&_plugin) noexcept +{ + this->dataPtr = std::move(_plugin.dataPtr); + return *this; +} diff --git a/src/Plugin_TEST.cc b/src/Plugin_TEST.cc new file mode 100644 index 000000000..00d5b6245 --- /dev/null +++ b/src/Plugin_TEST.cc @@ -0,0 +1,263 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include "sdf/parser.hh" +#include "sdf/Plugin.hh" +#include "sdf/Element.hh" + +///////////////////////////////////////////////// +TEST(DOMPlugin, Construction) +{ + sdf::Plugin plugin; + EXPECT_EQ(nullptr, plugin.Element()); + + EXPECT_TRUE(plugin.Name().empty()); + EXPECT_TRUE(plugin.Filename().empty()); + EXPECT_TRUE(plugin.Contents().empty()); + + plugin.SetName("my-plugin"); + EXPECT_EQ("my-plugin", plugin.Name()); + + plugin.SetFilename("filename.so"); + EXPECT_EQ("filename.so", plugin.Filename()); + + sdf::ElementPtr content(new sdf::Element); + content->SetName("an-element"); + plugin.InsertContent(content); + EXPECT_EQ(1u, plugin.Contents().size()); + + sdf::ElementPtr elem = plugin.ToElement(); + ASSERT_NE(nullptr, elem); + + ASSERT_NE(nullptr, elem->GetAttribute("name")); + EXPECT_EQ("my-plugin", elem->GetAttribute("name")->GetAsString()); + + ASSERT_NE(nullptr, elem->GetAttribute("filename")); + EXPECT_EQ("filename.so", elem->GetAttribute("filename")->GetAsString()); +} + +///////////////////////////////////////////////// +TEST(DOMPlugin, MoveConstructor) +{ + sdf::Plugin plugin; + plugin.SetName("pluginname"); + plugin.SetFilename("filename"); + + sdf::ElementPtr content(new sdf::Element); + content->SetName("an-element"); + plugin.InsertContent(content); + + sdf::Plugin plugin2(std::move(plugin)); + EXPECT_EQ("pluginname", plugin2.Name()); + EXPECT_EQ("filename", plugin2.Filename()); + ASSERT_EQ(1u, plugin2.Contents().size()); + EXPECT_EQ("an-element", plugin2.Contents()[0]->GetName()); +} + +///////////////////////////////////////////////// +TEST(DOMPlugin, CopyConstructor) +{ + sdf::Plugin plugin; + plugin.SetName("pluginname"); + plugin.SetFilename("filename"); + + sdf::Plugin plugin2(plugin); + EXPECT_EQ("pluginname", plugin2.Name()); + EXPECT_EQ("filename", plugin2.Filename()); + + EXPECT_EQ("pluginname", plugin.Name()); + EXPECT_EQ("filename", plugin.Filename()); +} + +///////////////////////////////////////////////// +TEST(DOMPlugin, CopyAssigmentOperator) +{ + sdf::Plugin plugin; + plugin.SetName("pluginname"); + plugin.SetFilename("filename"); + + sdf::Plugin plugin2; + plugin2 = plugin; + EXPECT_EQ("pluginname", plugin2.Name()); + EXPECT_EQ("filename", plugin2.Filename()); + + EXPECT_EQ("pluginname", plugin.Name()); + EXPECT_EQ("filename", plugin.Filename()); +} + +///////////////////////////////////////////////// +TEST(DOMPlugin, MoveAssignmentConstructor) +{ + sdf::Plugin plugin; + plugin.SetName("pluginname"); + plugin.SetFilename("filename"); + + sdf::Plugin plugin2; + plugin2 = std::move(plugin); + EXPECT_EQ("pluginname", plugin2.Name()); + EXPECT_EQ("filename", plugin2.Filename()); +} + +///////////////////////////////////////////////// +TEST(DOMPlugin, CopyAssignmentAfterMove) +{ + sdf::Plugin plugin; + plugin.SetName("pluginname"); + plugin.SetFilename("filename"); + + sdf::Plugin plugin2; + plugin2.SetName("pluginname2"); + plugin2.SetFilename("filename2"); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Plugin tmp = std::move(plugin); + plugin = plugin2; + plugin2 = tmp; + + EXPECT_EQ("pluginname", plugin2.Name()); + EXPECT_EQ("filename", plugin2.Filename()); + + EXPECT_EQ("pluginname2", plugin.Name()); + EXPECT_EQ("filename2", plugin.Filename()); +} + +///////////////////////////////////////////////// +TEST(DOMPlugin, Load) +{ + sdf::Plugin plugin; + sdf::Errors errors; + + // Null sdf + errors = plugin.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = plugin.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, plugin.Element()); + + sdf->SetName("plugin"); + + // Missing name and filename attribute + errors = plugin.Load(sdf); + ASSERT_EQ(2u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ATTRIBUTE_MISSING, errors[0].Code()); + EXPECT_EQ(sdf::ErrorCode::ATTRIBUTE_MISSING, errors[1].Code()); + + sdf->AddAttribute("name", "string", "__default__", true); + sdf->GetAttribute("name")->Set("my-plugin-name"); + + // Now just missing filename + errors = plugin.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ATTRIBUTE_MISSING, errors[0].Code()); + + sdf->AddAttribute("filename", "string", "__default__", true); + sdf->GetAttribute("filename")->Set("filename.so"); + + // No errors. + errors = plugin.Load(sdf); + ASSERT_TRUE(errors.empty()); +} + +///////////////////////////////////////////////// +TEST(DOMPlugin, LoadWithChildren) +{ + std::string pluginStr = R"( + + 3D View + false + docked + + ogre + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + +)"; + + std::string pluginStrWithSdf = std::string("") + + pluginStr + ""; + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("plugin.sdf", elem); + sdf::readString(pluginStrWithSdf, elem); + + sdf::Plugin plugin; + sdf::Errors errors; + errors = plugin.Load(elem); + ASSERT_EQ(0u, errors.size()); + + EXPECT_EQ("3D View", plugin.Name()); + EXPECT_EQ("MinimalScene", plugin.Filename()); + + // The elements should be the same + EXPECT_EQ(elem->ToString(""), plugin.Element()->ToString("")); + + sdf::ElementPtr toElem = plugin.ToElement(); + + // The elements should be the same + EXPECT_EQ(elem->ToString(""), toElem->ToString("")); + EXPECT_EQ(pluginStr, toElem->ToString("")); + + // Test plugin copy + sdf::Plugin plugin3; + plugin3 = plugin; + plugin.ClearContents(); + sdf::Plugin plugin4(plugin3); + + toElem = plugin3.ToElement(); + EXPECT_EQ(6u, plugin3.Contents().size()); + EXPECT_EQ(pluginStr, toElem->ToString("")); + + toElem = plugin4.ToElement(); + EXPECT_EQ(6u, plugin4.Contents().size()); + EXPECT_EQ(pluginStr, toElem->ToString("")); +} + +///////////////////////////////////////////////// +TEST(DOMPlugin, ToElement) +{ + sdf::Plugin plugin; + plugin.SetName("my-plugin"); + EXPECT_EQ("my-plugin", plugin.Name()); + + plugin.SetFilename("filename.so"); + EXPECT_EQ("filename.so", plugin.Filename()); + + sdf::ElementPtr content(new sdf::Element); + content->SetName("an-element"); + plugin.InsertContent(content); + EXPECT_EQ(1u, plugin.Contents().size()); + + sdf::ElementPtr elem = plugin.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Plugin plugin2; + plugin2.Load(elem); + + EXPECT_EQ(plugin.Name(), plugin2.Name()); + EXPECT_EQ(plugin.Filename(), plugin2.Filename()); + EXPECT_EQ(1u, plugin2.Contents().size()); + EXPECT_EQ("an-element", plugin2.Contents()[0]->GetName()); +} From de356cd6da967624a0f8a517499bcfad79f6f655 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 20 Dec 2021 14:02:51 -0600 Subject: [PATCH 58/60] Refactor FrameSemantics.cc (#764) The buildFrameAttachedToGraph and buildPoseRelativeToGraph have overloads for the type of parent element, but the code in each overload is very similar to each other. This refactors these functions so there's less code duplication. Signed-off-by: Addisu Z. Taddese Co-authored-by: Steve Peters --- src/FrameSemantics.cc | 1655 ++++++++++------------------- src/ign_TEST.cc | 10 +- test/integration/frame.cc | 4 +- test/integration/interface_api.cc | 2 +- 4 files changed, 582 insertions(+), 1089 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index a07ac6906..4df943448 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -198,17 +198,6 @@ FindSinkVertex( return PairType(vertex, edges); } -///////////////////////////////////////////////// -std::pair - modelCanonicalLinkAndRelativeName(const Model *_model) -{ - if (nullptr == _model) - { - return std::make_pair(nullptr, ""); - } - return _model->CanonicalLinkAndRelativeName(); -} - ///////////////////////////////////////////////// /// \brief Resolve the pose of a model taking into account the placement frame /// attribute. This function is used to calculate the pose of the edge between a @@ -282,289 +271,535 @@ static Errors resolveModelPoseWithPlacementFrame( return errors; } -///////////////////////////////////////////////// -Errors buildFrameAttachedToGraph( - ScopedGraph &_out, const Model *_model, bool _isRoot) +/// \brief Base struct the contains a few common members. These structs provide +/// a common API used by the build*Graph functions to access the various +/// attributes and retrieve children of regular DOM objects and Interface +/// Elements. +struct WrapperBase { - Errors errors; + /// \brief Name of the entity + const std::string name; + /// \brief Element type, such as Model, Link, or Interface Frame. + const std::string elementType; + /// \brief Frame type used in the FrameAttachedTo or PoseRelativeTo graphs. + const FrameType frameType; +}; + +/// \brief Wrapper for sdf::Link and sdf::InterfaceLink +struct LinkWrapper : public WrapperBase +{ + /// \brief Constructor that takes an sdf::Link + explicit LinkWrapper(const sdf::Link &_link) + : WrapperBase{_link.Name(), "Link", FrameType::LINK}, + rawPose(_link.RawPose()), + rawRelativeTo(_link.PoseRelativeTo()), + relativeTo(rawRelativeTo) + { + } - if (!_model) + /// \brief Constructor that takes an sdf::InterfaceLink + explicit LinkWrapper(const sdf::InterfaceLink &_ifaceLink) + : WrapperBase{_ifaceLink.Name(), "Interface Link", FrameType::LINK}, + rawPose(_ifaceLink.PoseInModelFrame()), + rawRelativeTo("__model__"), + relativeTo(rawRelativeTo) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid sdf::Model pointer."}); - return errors; } - else if (!_model->Element()) + + /// \brief Raw pose of the entity. + const ignition::math::Pose3d rawPose; + /// \brief The //pose/@relative_to attribute. + const std::string rawRelativeTo; + /// \brief The final @relative_to attribute. This is the same as rawRelativeTo + /// for links and interface links. + const std::string relativeTo; +}; + +/// \brief Wrapper for sdf::Frame and sdf::InterfaceFrame +struct FrameWrapper : public WrapperBase +{ + /// \brief Constructor that takes an sdf::Frame + explicit FrameWrapper(const sdf::Frame &_frame) + : WrapperBase{_frame.Name(), "Frame", FrameType::FRAME}, + rawPose(_frame.RawPose()), + rawRelativeTo(_frame.PoseRelativeTo()), + attachedTo(_frame.AttachedTo()), + relativeTo(rawRelativeTo.empty() ? attachedTo : rawRelativeTo) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid model element in sdf::Model."}); - return errors; } - else if (_model->LinkCount() == 0 && _model->ModelCount() == 0 && - _model->InterfaceModelCount() == 0 && !_model->Static()) + + /// \brief Constructor that takes an sdf::InterfaceFrame + explicit FrameWrapper(const sdf::InterfaceFrame &_ifaceFrame) + : WrapperBase{_ifaceFrame.Name(), "Interface Frame", FrameType::FRAME}, + rawPose(_ifaceFrame.PoseInAttachedToFrame()), + rawRelativeTo(_ifaceFrame.AttachedTo()), + attachedTo(_ifaceFrame.AttachedTo()), + relativeTo(attachedTo) + { - errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, - "A model must have at least one link."}); - return errors; } - auto frameType = - _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; + /// \brief Raw pose of the entity. + const ignition::math::Pose3d rawPose; + /// \brief The //pose/@relative_to attribute. + const std::string rawRelativeTo; + /// \brief The //frame/@attached_to attribute. + const std::string attachedTo; + /// \brief The final @relative_to attribute. For sdf::Frame, this is set to + /// the attachedTo if the rawRelativeTo is empty. For sdf::InterfaceFrame, + /// it's always set to attachedTo. + const std::string relativeTo; +}; - const std::string scopeContextName = "__model__"; +/// \brief Wrapper for sdf::Joint and sdf::InterfaceJoint +struct JointWrapper : public WrapperBase +{ + /// \brief Constructor that takes an sdf::Joint + explicit JointWrapper(const sdf::Joint &_joint) + : WrapperBase{_joint.Name(), "Joint", FrameType::JOINT}, + rawPose(_joint.RawPose()), + rawRelativeTo(_joint.PoseRelativeTo()), + childName(_joint.ChildLinkName()), + relativeTo(rawRelativeTo.empty() ? childName : rawRelativeTo) + { + } + + /// \brief Constructor that takes an sdf::InterfaceJoint + explicit JointWrapper(const sdf::InterfaceJoint &_ifaceJoint) + : WrapperBase{_ifaceJoint.Name(), "Interface Joint", FrameType::JOINT}, + rawPose(_ifaceJoint.PoseInChildFrame()), + rawRelativeTo(_ifaceJoint.ChildName()), + childName(_ifaceJoint.ChildName()), + relativeTo(childName) + { + } + + /// \brief Raw pose of the entity. + const ignition::math::Pose3d rawPose; + /// \brief The //pose/@relative_to attribute. + const std::string rawRelativeTo; + /// \brief The name of the child frame (i.e. content of //joint/child). + const std::string childName; + /// \brief The final @relative_to attribute. For sdf::Joint, this is set to + /// the childName if the rawRelativeTo is empty. For sdf::InterfaceJoint, it's + /// always set to childName. + const std::string relativeTo; +}; + +/// \brief Wrapper for sdf::Model and sdf::InterfaceModel +struct ModelWrapper : public WrapperBase +{ + /// \brief Constructor that takes an sdf::Model + explicit ModelWrapper(const sdf::Model &_model) + : WrapperBase{_model.Name(), "Model", + _model.Static() ? FrameType::STATIC_MODEL + : FrameType::MODEL}, + rawPose(_model.RawPose()), + rawRelativeTo(_model.PoseRelativeTo()), + relativeTo(rawRelativeTo), + canonicalLinkName(_model.CanonicalLinkAndRelativeName().second), + placementFrameName(_model.PlacementFrameName()), + isStatic(_model.Static()) + { + for (uint64_t i = 0; i < _model.LinkCount(); ++i) + { + this->links.emplace_back(*_model.LinkByIndex(i)); + } + for (uint64_t i = 0; i < _model.FrameCount(); ++i) + { + this->frames.emplace_back(*_model.FrameByIndex(i)); + } + for (uint64_t i = 0; i < _model.JointCount(); ++i) + { + this->joints.emplace_back(*_model.JointByIndex(i)); + } + for (uint64_t i = 0; i < _model.ModelCount(); ++i) + { + this->models.emplace_back(*_model.ModelByIndex(i)); + } + for (uint64_t i = 0; i < _model.InterfaceModelCount(); ++i) + { + this->models.emplace_back(*_model.InterfaceModelNestedIncludeByIndex(i), + *_model.InterfaceModelByIndex(i)); + } + } - auto rootId = ignition::math::graph::kNullId; - if (_isRoot) + /// \brief Constructor that takes an sdf::NestedInclude and + /// sdf::InterfaceModel. + explicit ModelWrapper(const NestedInclude &_nestedInclude, + const sdf::InterfaceModel &_ifaceModel) + : WrapperBase{_ifaceModel.Name(), "Interface Model", + _ifaceModel.Static() ? FrameType::STATIC_MODEL + : FrameType::MODEL}, + rawPose(_ifaceModel.ModelFramePoseInParentFrame()), + rawRelativeTo(_nestedInclude.IncludePoseRelativeTo().value_or("")), + relativeTo(rawRelativeTo), + canonicalLinkName(_ifaceModel.CanonicalLinkName()), + placementFrameName(_nestedInclude.PlacementFrame().value_or("")), + isStatic(_ifaceModel.Static()) { - // The __root__ vertex identifies the scope that contains a root level - // model. In the PoseRelativeTo graph, this vertex is connected to the model - // with an edge that holds the value of //model/pose. However, in the - // FrameAttachedTo graph, the vertex is disconnected because nothing is - // attached to it. Since only links and static models are allowed to be - // disconnected in this graph, the STATIC_MODEL was chosen as its frame - // type. A different frame type could potentially be used, but that adds - // more complexity to the validateFrameAttachedToGraph code. - _out = _out.AddScopeVertex( - "", "__root__", scopeContextName, sdf::FrameType::STATIC_MODEL); - rootId = _out.ScopeVertexId(); + this->AddInterfaceChildren(_ifaceModel); } - const auto modelId = _out.AddVertex(_model->Name(), frameType).Id(); + /// \brief Constructor that takes an sdf::InterfaceModel. These are + /// InterfaceModels nested under other InterfaceModels. + explicit ModelWrapper(const sdf::InterfaceModel &_ifaceModel) + : WrapperBase{_ifaceModel.Name(), "Interface Model", + _ifaceModel.Static() ? FrameType::STATIC_MODEL + : FrameType::MODEL}, + rawPose(_ifaceModel.ModelFramePoseInParentFrame()), + rawRelativeTo(""), + relativeTo(rawRelativeTo), + canonicalLinkName(_ifaceModel.CanonicalLinkName()), + placementFrameName(""), + isStatic(_ifaceModel.Static()) + { + this->AddInterfaceChildren(_ifaceModel); + } - auto outModel = _out.AddScopeVertex( - _model->Name(), scopeContextName, scopeContextName, frameType); - const auto modelFrameId = outModel.ScopeVertexId(); + /// \brief Raw pose of the entity. + const ignition::math::Pose3d rawPose; + /// \brief The //pose/@relative_to attribute. + const std::string rawRelativeTo; + /// \brief The final @relative_to attribute. This is set to rawRelativeTo for + /// sdf::Model and sdf::InterfaceModel. + const std::string relativeTo; + /// \brief The name of the resolved canonical link. + const std::string canonicalLinkName; + /// \brief The name of the placement frame. + const std::string placementFrameName; + /// \brief Whether the model/interface model is static. + const bool isStatic; - // Add an aliasing edge from the model vertex to the - // corresponding vertex in the FrameAttachedTo graph of the child model, - auto &edge = outModel.AddEdge({modelId, modelFrameId}, true); - // Set the edge weight to 0 to indicate that this is an aliasing edge. - edge.SetWeight(0); + /// \brief Children links and interface links. + std::vector links; + /// \brief Children frames and interface frames. + std::vector frames; + /// \brief Children joints and interface joints. + std::vector joints; + /// \brief Children nested models and interface models. + std::vector models; - // add link vertices - for (uint64_t l = 0; l < _model->LinkCount(); ++l) + /// \brief Helper function to add children of interface models. + private: void AddInterfaceChildren(const sdf::InterfaceModel &_ifaceModel) { - auto link = _model->LinkByIndex(l); - if (outModel.Count(link->Name()) > 0) + for (const auto &item : _ifaceModel.Links()) { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Link with non-unique name [" + link->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; + this->links.emplace_back(item); + } + for (const auto &item : _ifaceModel.Frames()) + { + this->frames.emplace_back(item); + } + for (const auto &item : _ifaceModel.Joints()) + { + this->joints.emplace_back(item); + } + for (const auto &item : _ifaceModel.NestedModels()) + { + this->models.emplace_back(*item); } - outModel.AddVertex(link->Name(), sdf::FrameType::LINK); } +}; - // add joint vertices - for (uint64_t j = 0; j < _model->JointCount(); ++j) +/// \brief Wrapper for sdf::World +struct WorldWrapper : public WrapperBase +{ + /// \brief Constructor that takes an sdf::World + explicit WorldWrapper(const sdf::World &_world) + : WrapperBase{_world.Name(), "World", FrameType::WORLD} { - auto joint = _model->JointByIndex(j); - if (outModel.Count(joint->Name()) > 0) + for (uint64_t i = 0; i < _world.FrameCount(); ++i) { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Joint with non-unique name [" + joint->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; + this->frames.emplace_back(*_world.FrameByIndex(i)); + } + for (uint64_t i = 0; i < _world.ModelCount(); ++i) + { + this->models.emplace_back(*_world.ModelByIndex(i)); + } + for (uint64_t i = 0; i < _world.InterfaceModelCount(); ++i) + { + this->models.emplace_back(*_world.InterfaceModelNestedIncludeByIndex(i), + *_world.InterfaceModelByIndex(i)); } - outModel.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); } - // add frame vertices - for (uint64_t f = 0; f < _model->FrameCount(); ++f) + /// \brief Children frames. + std::vector frames; + /// \brief Children models and interface models. + std::vector models; +}; + +///////////////////////////////////////////////// +/// \brief Add vertices to either Frame attached to or Pose graph. If the child +/// element is a model, it calls the corresponding build*Graph function. +/// \tparam ElementT The type of Element. This must be a class that derives from +/// WrapperBase. +/// \tparam GraphT Type of Graph. Either PoseRelativeToGraph or +/// FrameAttachedToGraph. +/// \param[in,out] _out The graph to which vertices will be added. +/// \param[in] _items List of Elements such as links, joints, etc for which +/// vertices will be created in the graph. +/// \param[in] _parent Parent element of the items in `_items`. +/// \param[out] _errors Errors encountered while adding vertices. +template +void addVerticesToGraph(ScopedGraph &_out, + const std::vector &_items, + const WrapperBase &_parent, Errors &_errors) +{ + for (const auto &item : _items) { - auto frame = _model->FrameByIndex(f); - if (outModel.Count(frame->Name()) > 0) + if (_out.Count(item.name) > 0) { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Frame with non-unique name [" + frame->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); + _errors.emplace_back(ErrorCode::DUPLICATE_NAME, item.elementType + + " with non-unique name [" + item.name + "] detected in " + + lowercase(_parent.elementType) + " with name [" + + _parent.name + "]."); continue; } - outModel.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); + if constexpr (std::is_same_v) + { + if constexpr (std::is_same_v) + { + auto nestedErrors = wrapperBuildFrameAttachedToGraph(_out, item, false); + _errors.insert(_errors.end(), nestedErrors.begin(), nestedErrors.end()); + } + else + { + auto nestedErrors = wrapperBuildPoseRelativeToGraph(_out, item, false); + _errors.insert(_errors.end(), nestedErrors.begin(), nestedErrors.end()); + } + } + else + { + _out.AddVertex(item.name, item.frameType); + } } +} - // add nested model vertices - for (uint64_t m = 0; m < _model->ModelCount(); ++m) +///////////////////////////////////////////////// +/// \brief Add edges to the PoseRelativeTo graph. +/// \tparam ElementT The type of Element. This must be a class that derives from +/// WrapperBase. +/// \param[in,out] _out The PoseRelativeTo graph to which edges will be added. +/// \param[in] _items List of Elements such as links, joints, etc for which +/// edges will be created in the graph. +/// \param[in] _parent Parent element of the items in `_items`. +/// \param[out] _errors Errors encountered while adding edges. +template +void addEdgesToGraph(ScopedGraph &_out, + const std::vector &_items, + const WrapperBase &_parent, Errors &_errors) +{ + for (const auto &item : _items) { - auto nestedModel = _model->ModelByIndex(m); - if (outModel.Count(nestedModel->Name()) > 0) + const std::string &relativeTo = item.relativeTo; + const ignition::math::Pose3d poseInRelativeTo = item.rawPose; + + auto itemId = _out.VertexIdByName(item.name); + auto relativeToId = _out.ScopeVertexId(); + std::string typeForErrorMsg = "relative_to"; + ErrorCode errorCode = ErrorCode::POSE_RELATIVE_TO_INVALID; + + if constexpr (std::is_same_v) { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Nested model with non-unique name [" + nestedModel->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; + if (item.rawRelativeTo.empty()) + { + typeForErrorMsg = "attached_to"; + errorCode = ErrorCode::FRAME_ATTACHED_TO_INVALID; + } } - auto nestedErrors = buildFrameAttachedToGraph(outModel, nestedModel, false); - errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); - } - // add nested interface model vertices - for (uint64_t m = 0; m < _model->InterfaceModelCount(); ++m) - { - auto nestedIfaceModel = _model->InterfaceModelByIndex(m); - if (outModel.Count(nestedIfaceModel->Name()) > 0) + if (!relativeTo.empty()) { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Nested interface model with non-unique name [" + - nestedIfaceModel->Name() + "] detected in model with name [" + - _model->Name() + "]."}); - continue; + // look for vertex in graph that matches relative_to value + if (_out.Count(relativeTo) != 1) + { + std::stringstream errMsg; + errMsg << typeForErrorMsg << " name[" << relativeTo << "] specified by " + << lowercase(item.elementType) << " with name[" << item.name + << "] does not match a"; + if (_parent.frameType == FrameType::WORLD) + { + errMsg << " model or frame name "; + } + else + { + errMsg << " nested model, link, joint, or frame name "; + } + errMsg << "in " + lowercase(_parent.elementType) + " with name[" + + _parent.name + "]."; + + _errors.push_back({errorCode, errMsg.str()}); + continue; + } + + relativeToId = _out.VertexIdByName(relativeTo); + if (item.name == relativeTo) + { + _errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, + "relative_to name[" + relativeTo + + "] is identical to " + lowercase(item.elementType) + " name[" + + item.name + "], causing a graph cycle in " + + lowercase(_parent.elementType) + " with name[" + + _parent.name + "]."}); + } + } + + if constexpr (std::is_same_v) + { + ignition::math::Pose3d resolvedModelPose = item.rawPose; + + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( + item.rawPose, item.placementFrameName, + _out.ChildModelScope(item.name), resolvedModelPose); + _errors.insert(_errors.end(), resolveErrors.begin(), resolveErrors.end()); + + _out.AddEdge({relativeToId, itemId}, resolvedModelPose); + } + else + { + _out.AddEdge({relativeToId, itemId}, poseInRelativeTo); } - auto nestedErrors = buildFrameAttachedToGraph(outModel, nestedIfaceModel); - errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); } +} - // add edges from joint to child frames - for (uint64_t j = 0; j < _model->JointCount(); ++j) +///////////////////////////////////////////////// +/// \brief Add Joint and InterfaceJoint edges to the FrameAttachedTo graph +/// \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[out] _errors Errors encountered while adding edges. +void addEdgesToGraph(ScopedGraph &_out, + const std::vector &_joints, + const ModelWrapper &_model, Errors &_errors) +{ + for (const auto &joint : _joints) { - auto joint = _model->JointByIndex(j); - auto jointId = outModel.VertexIdByName(joint->Name()); - auto childFrameName = joint->ChildLinkName(); - if (outModel.Count(childFrameName) != 1) + auto jointId = _out.VertexIdByName(joint.name); + + if (_out.Count(joint.childName) != 1) { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "Child frame with name[" + childFrameName + - "] specified by joint with name[" + joint->Name() + - "] not found in model with name[" + _model->Name() + "]."}); + _errors.push_back( + {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 + "]."}); continue; } - auto childFrameId = outModel.VertexIdByName(childFrameName); - outModel.AddEdge({jointId, childFrameId}, true); + auto childFrameId = _out.VertexIdByName(joint.childName); + _out.AddEdge({jointId, childFrameId}, true); } +} - // add frame edges - for (uint64_t f = 0; f < _model->FrameCount(); ++f) +///////////////////////////////////////////////// +/// \brief Add Frame and InterfaceFrame edges to the FrameAttachedTo graph +/// \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[out] _errors Errors encountered while adding edges. +void addEdgesToGraph(ScopedGraph &_out, + const std::vector &_frames, + const WrapperBase &_parent, Errors &_errors) +{ + for (const auto &frame : _frames) { - auto frame = _model->FrameByIndex(f); - auto frameId = outModel.VertexIdByName(frame->Name()); + auto frameId = _out.VertexIdByName(frame.name); // look for vertex in graph that matches attached_to value - std::string attachedTo = frame->AttachedTo(); + std::string attachedTo = frame.attachedTo; if (attachedTo.empty()) { - // if the attached-to name is empty, use the scope context name - attachedTo = scopeContextName; - } - if (outModel.Count(attachedTo) != 1) - { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, - "attached_to name[" + attachedTo + - "] specified by frame with name[" + frame->Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; - } - auto attachedToId = outModel.VertexIdByName(attachedTo); - bool edgeData = true; - if (frame->Name() == frame->AttachedTo()) - { - // set edgeData to false if attaches to itself, since this is invalid - edgeData = false; - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_CYCLE, - "attached_to name[" + attachedTo + - "] is identical to frame name[" + frame->Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); + // if the attached-to name is empty, use the default attachedTo + attachedTo = _out.ScopeContextName(); } - outModel.AddEdge({frameId, attachedToId}, edgeData); - } - // identify canonical link, which may be nested - const auto[canonicalLink, canonicalLinkName] = - modelCanonicalLinkAndRelativeName(_model); - if (!_model->Static()) - { - if (nullptr == canonicalLink) + if (_out.Count(attachedTo) != 1) { - if (canonicalLinkName.empty()) + std::stringstream errMsg; + errMsg << "attached_to name[" << attachedTo << "] specified by " + << lowercase(frame.elementType) << " with name[" << frame.name + << "] does not match a"; + if (_parent.frameType == FrameType::WORLD) { - if (_model->ModelCount() == 0 && _model->InterfaceModelCount() == 0) - { - errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, - "A model must have at least one link."}); - } - else - { - // The canonical link was not found, but the model could have a - // descendant that has a static model, so simply create an edge to the - // first model and let the attached_to frame resolution take care of - // finding the canonical link - // - std::string firstChildModelName = ""; - if (_model->ModelCount() > 0) - { - firstChildModelName = _model->ModelByIndex(0)->Name(); - } - else - { - firstChildModelName = _model->InterfaceModelByIndex(0)->Name(); - } - auto firstChildModelId = outModel.VertexIdByName(firstChildModelName); - outModel.AddEdge({modelFrameId, firstChildModelId}, true); - } + errMsg << " model or frame name "; } else { - // Search for the vertex in case the canonical link is an InterfaceLink - auto canonicalLinkId = outModel.VertexIdByName(canonicalLinkName); - if (ignition::math::graph::kNullId != canonicalLinkId) - { - outModel.AddEdge({modelFrameId, canonicalLinkId}, true); - } - else - { - errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID, - "canonical_link with name[" + canonicalLinkName + - "] not found in model with name[" + _model->Name() + "]."}); - } + errMsg << " nested model, link, joint, or frame name "; } - // return early - return errors; + errMsg << "in " + lowercase(_parent.elementType) + " with name[" + + _parent.name + "]."; + + _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, errMsg.str()}); + continue; } - else + + auto attachedToId = _out.VertexIdByName(attachedTo); + bool edgeData = true; + if (frame.name == frame.attachedTo) { - // Add an edge from the implicit model frame to the canonical link found. - auto linkId = outModel.VertexIdByName(canonicalLinkName); - outModel.AddEdge({modelFrameId, linkId}, true); + // set edgeData to false if attaches to itself, since this is invalid + edgeData = false; + _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_CYCLE, + "attached_to name[" + attachedTo + + "] is identical to frame name[" + frame.attachedTo + + "], causing a graph cycle in " + lowercase(_parent.elementType) + + " with name[" + _parent.name + "]."}); } + _out.AddEdge({frameId, attachedToId}, edgeData); } - - return errors; } ///////////////////////////////////////////////// -Errors buildFrameAttachedToGraph(ScopedGraph &_out, - InterfaceModelConstPtr _model) +/// \brief Helper function that actually build a FrameAttachedToGraph given a +/// wrapped Model or Interface Model. +/// \param[out] _out Graph object to write. +/// \param[in] _model Wrapped Model or Interface model from which to build +/// attached_to graph. +/// \param[in] _isRoot True if the model is a standalone model, i.e, +/// //sdf/model. This is not relevant if the _model is a wrapped Interface +/// Model. +/// \return Errors. +Errors wrapperBuildFrameAttachedToGraph(ScopedGraph &_out, + const ModelWrapper &_model, + bool _isRoot) { Errors errors; - if (!_model) - { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid sdf::InterfaceModel pointer."}); - return errors; - } - else if (_model->Links().size() == 0u && _model->NestedModels().size() == 0 && - !_model->Static()) + if (_model.links.size() == 0u && _model.models.size() == 0 && + !_model.isStatic) { errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, - "Model with name[" + _model->Name() + - "] must have at least one link."}); + "A model must have at least one link."}); return errors; } - auto frameType = - _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; + auto frameType = _model.frameType; const std::string scopeContextName = "__model__"; - const auto modelId = _out.AddVertex(_model->Name(), frameType).Id(); + auto rootId = ignition::math::graph::kNullId; + if (_isRoot) + { + // The __root__ vertex identifies the scope that contains a root level + // model. In the PoseRelativeTo graph, this vertex is connected to the model + // with an edge that holds the value of //model/pose. However, in the + // FrameAttachedTo graph, the vertex is disconnected because nothing is + // attached to it. Since only links and static models are allowed to be + // disconnected in this graph, the STATIC_MODEL was chosen as its frame + // type. A different frame type could potentially be used, but that adds + // more complexity to the validateFrameAttachedToGraph code. + _out = _out.AddScopeVertex( + "", "__root__", scopeContextName, sdf::FrameType::STATIC_MODEL); + rootId = _out.ScopeVertexId(); + } + + const auto modelId = _out.AddVertex(_model.name, frameType).Id(); auto outModel = _out.AddScopeVertex( - _model->Name(), scopeContextName, scopeContextName, frameType); + _model.name, scopeContextName, scopeContextName, frameType); const auto modelFrameId = outModel.ScopeVertexId(); // Add an aliasing edge from the model vertex to the @@ -574,128 +809,36 @@ Errors buildFrameAttachedToGraph(ScopedGraph &_out, edge.SetWeight(0); // add link vertices - for (const auto &link : _model->Links()) - { - if (outModel.Count(link.Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Link with non-unique name [" + link.Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - outModel.AddVertex(link.Name(), sdf::FrameType::LINK); - } + addVerticesToGraph(outModel, _model.links, _model, errors); // add joint vertices - for (const auto &joint : _model->Joints()) - { - if (outModel.Count(joint.Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Joint with non-unique name [" + joint.Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - outModel.AddVertex(joint.Name(), sdf::FrameType::JOINT); - } + addVerticesToGraph(outModel, _model.joints, _model, errors); // add frame vertices - for (const auto &frame : _model->Frames()) - { - if (outModel.Count(frame.Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Frame with non-unique name [" + frame.Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - outModel.AddVertex(frame.Name(), sdf::FrameType::FRAME); - } + addVerticesToGraph(outModel, _model.frames, _model, errors); // add nested model vertices - for (const auto &nestedIfaceModel : _model->NestedModels()) - { - if (outModel.Count(nestedIfaceModel->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Nested model with non-unique name [" + nestedIfaceModel->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - auto nestedErrors = buildFrameAttachedToGraph(outModel, nestedIfaceModel); - errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); - } + addVerticesToGraph(outModel, _model.models, _model, errors); // add edges from joint to child frames - for (const auto &joint : _model->Joints()) - { - auto jointId = outModel.VertexIdByName(joint.Name()); - const auto &childFrameName = joint.ChildName(); - if (outModel.Count(childFrameName) != 1) - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "Child frame with name[" + childFrameName + - "] specified by joint with name[" + joint.Name() + - "] not found in model with name[" + _model->Name() + "]."}); - continue; - } - auto childFrameId = outModel.VertexIdByName(childFrameName); - outModel.AddEdge({jointId, childFrameId}, true); - } + addEdgesToGraph(outModel, _model.joints, _model, errors); // add frame edges - for (const auto &frame : _model->Frames()) - { - auto frameId = outModel.VertexIdByName(frame.Name()); - // look for vertex in graph that matches attached_to value - std::string attachedTo = frame.AttachedTo(); - if (attachedTo.empty()) - { - // if the attached-to name is empty, use the scope context name - attachedTo = scopeContextName; - } - if (outModel.Count(attachedTo) != 1) - { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, - "attached_to name[" + attachedTo + - "] specified by frame with name[" + frame.Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; - } - auto attachedToId = outModel.VertexIdByName(attachedTo); - bool edgeData = true; - if (frame.Name() == frame.AttachedTo()) - { - // set edgeData to false if attaches to itself, since this is invalid - edgeData = false; - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_CYCLE, - "attached_to name[" + attachedTo + - "] is identical to frame name[" + frame.Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); - } - outModel.AddEdge({frameId, attachedToId}, edgeData); - } + addEdgesToGraph(outModel, _model.frames, _model, errors); // identify canonical link, which may be nested - const std::string canonicalLinkName = _model->CanonicalLinkName(); + const std::string canonicalLinkName = _model.canonicalLinkName; const auto canonicalLinkId = outModel.VertexIdByName(canonicalLinkName); - if (!_model->Static()) + if (!_model.isStatic) { if (ignition::math::graph::kNullId == canonicalLinkId) { if (canonicalLinkName.empty()) { - if (_model->NestedModels().size() == 0u) + if (_model.models.size() == 0u) { errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, - "Interface model with name[" + _model->Name() + - "] must have at least one link."}); + "A model must have at least one link."}); } else { @@ -704,15 +847,15 @@ Errors buildFrameAttachedToGraph(ScopedGraph &_out, // first model and let the attached_to frame resolution take care of // finding the canonical link auto firstChildModelId = - outModel.VertexIdByName(_model->NestedModels().front()->Name()); - outModel.AddEdge({modelFrameId, firstChildModelId }, true); + outModel.VertexIdByName(_model.models.front().name); + outModel.AddEdge({modelFrameId, firstChildModelId}, true); } } else { errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID, "canonical_link with name[" + canonicalLinkName + - "] not found in model with name[" + _model->Name() + "]."}); + "] not found in model with name[" + _model.name + "]."}); } // return early return errors; @@ -726,25 +869,40 @@ Errors buildFrameAttachedToGraph(ScopedGraph &_out, return errors; } - ///////////////////////////////////////////////// Errors buildFrameAttachedToGraph( - ScopedGraph &_out, const World *_world) + ScopedGraph &_out, const Model *_model, bool _isRoot) { - Errors errors; - - if (!_world) + if (!_model) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid sdf::World pointer."}); - return errors; + return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::Model pointer."}}; } - else if (!_world->Element()) + else if (!_model->Element()) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid world element in sdf::World."}); - return errors; + return Errors{ + {ErrorCode::ELEMENT_INVALID, "Invalid model element in sdf::Model."}}; } + return wrapperBuildFrameAttachedToGraph(_out, ModelWrapper(*_model), _isRoot); +} + +///////////////////////////////////////////////// +Errors buildFrameAttachedToGraph(ScopedGraph &_out, + const InterfaceModel *_model) +{ + if (!_model) + { + return Errors{ + {ErrorCode::ELEMENT_INVALID, "Invalid sdf::InterfaceModel pointer."}}; + } + return wrapperBuildFrameAttachedToGraph(_out, ModelWrapper(*_model), false); +} + + +///////////////////////////////////////////////// +Errors buildFrameAttachedToGraph( + ScopedGraph &_out, const WorldWrapper &_world) +{ + Errors errors; // add implicit world frame vertex first const std::string scopeContextName = "world"; @@ -752,117 +910,49 @@ Errors buildFrameAttachedToGraph( "", scopeContextName, scopeContextName, sdf::FrameType::WORLD); // add model vertices - for (uint64_t m = 0; m < _world->ModelCount(); ++m) - { - auto model = _world->ModelByIndex(m); - if (_out.Count(model->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Model with non-unique name [" + model->Name() + - "] detected in world with name [" + _world->Name() + - "]."}); - continue; - } - auto modelErrors = buildFrameAttachedToGraph(_out, model, false); - errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); - } - - // add interface model vertices - for (uint64_t im = 0; im < _world->InterfaceModelCount(); ++im) - { - auto ifaceModel = _world->InterfaceModelByIndex(im); - if (_out.Count(ifaceModel->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Interface Model with non-unique name [" + ifaceModel->Name() + - "] detected in world with name [" + _world->Name() + - "]."}); - continue; - } - auto modelErrors = buildFrameAttachedToGraph(_out, ifaceModel); - errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); - } + addVerticesToGraph(_out, _world.models, _world, errors); // add frame vertices - for (uint64_t f = 0; f < _world->FrameCount(); ++f) - { - auto frame = _world->FrameByIndex(f); - if (_out.Count(frame->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Frame with non-unique name [" + frame->Name() + - "] detected in world with name [" + _world->Name() + - "]."}); - continue; - } - _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - } + addVerticesToGraph(_out, _world.frames, _world, errors); // add frame edges - for (uint64_t f = 0; f < _world->FrameCount(); ++f) - { - auto frame = _world->FrameByIndex(f); - auto frameId = _out.VertexIdByName(frame->Name()); - // look for vertex in graph that matches attached_to value - std::string attachedTo = frame->AttachedTo(); - if (attachedTo.empty()) - { - // if the attached-to name is empty, use the scope context name - attachedTo = scopeContextName; - if (_out.Count(scopeContextName) != 1) - { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error: scope frame[" + - scopeContextName + "] not found in map."}); - continue; - } - } - if (_out.Count(attachedTo) != 1) - { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, - "attached_to name[" + attachedTo + - "] specified by frame with name[" + frame->Name() + - "] does not match a model or frame name " - "in world with name[" + _world->Name() + "]."}); - continue; - } - auto attachedToId = _out.VertexIdByName(attachedTo); - bool edgeData = true; - if (frame->Name() == frame->AttachedTo()) - { - // set edgeData to false if attaches to itself, since this is invalid - edgeData = false; - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_CYCLE, - "attached_to name[" + attachedTo + - "] is identical to frame name[" + frame->Name() + - "], causing a graph cycle " - "in world with name[" + _world->Name() + "]."}); - } - _out.AddEdge({frameId, attachedToId}, edgeData); - } + addEdgesToGraph(_out, _world.frames, _world, errors); return errors; } ///////////////////////////////////////////////// -Errors buildPoseRelativeToGraph( - ScopedGraph &_out, const Model *_model, bool _isRoot) +Errors buildFrameAttachedToGraph( + ScopedGraph &_out, const World *_world) { - Errors errors; - - if (!_model) + if (!_world) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid sdf::Model pointer."}); - return errors; + return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::World pointer."}}; } - else if (!_model->Element()) + else if (!_world->Element()) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid model element in sdf::Model."}); - return errors; + return Errors{ + {ErrorCode::ELEMENT_INVALID, "Invalid world element in sdf::World."}}; } + return buildFrameAttachedToGraph(_out, WorldWrapper(*_world)); +} + +///////////////////////////////////////////////// +/// \brief Helper function that actually builds the PoseRelativeToGraph given a +/// wrapped Model or Interface model. +/// \param[out] _out Graph object to write. +/// \param[in] _model Wrapped Model or Interface model from which to build +/// relative_to graph. +/// \param[in] _isRoot True if the model is a standalone model, i.e, +/// //sdf/model. This is not relevant if the _model is a wrapped Interface +/// Model. +/// \return Errors. +Errors wrapperBuildPoseRelativeToGraph(ScopedGraph &_out, + const ModelWrapper &_model, bool _isRoot) +{ + Errors errors; + const std::string scopeContextName = "__model__"; auto rootId = ignition::math::graph::kNullId; // add the model frame vertex first @@ -872,8 +962,8 @@ Errors buildPoseRelativeToGraph( "", "__root__", scopeContextName, sdf::FrameType::MODEL); rootId = _out.ScopeVertexId(); } - auto modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); - auto outModel = _out.AddScopeVertex(_model->Name(), scopeContextName, + auto modelId = _out.AddVertex(_model.name, sdf::FrameType::MODEL).Id(); + auto outModel = _out.AddScopeVertex(_model.name, scopeContextName, scopeContextName, sdf::FrameType::MODEL); auto modelFrameId = outModel.ScopeVertexId(); @@ -884,311 +974,32 @@ Errors buildPoseRelativeToGraph( // Set the edge weight to 0 to indicate that this is an aliasing edge. edge.SetWeight(0); - // add link vertices and default edge if relative_to is empty - for (uint64_t l = 0; l < _model->LinkCount(); ++l) - { - auto link = _model->LinkByIndex(l); - if (outModel.Count(link->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Link with non-unique name [" + link->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - auto linkId = - outModel.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); - - if (link->PoseRelativeTo().empty()) - { - // relative_to is empty, so add edge from implicit model frame to link - outModel.AddEdge({modelFrameId, linkId}, link->RawPose()); - } - } + // add link vertices + addVerticesToGraph(outModel, _model.links, _model, errors); // add joint vertices - for (uint64_t j = 0; j < _model->JointCount(); ++j) - { - auto joint = _model->JointByIndex(j); - if (outModel.Count(joint->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Joint with non-unique name [" + joint->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - outModel.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); - } + addVerticesToGraph(outModel, _model.joints, _model, errors); - // add frame vertices and default edge if both - // relative_to and attached_to are empty - for (uint64_t f = 0; f < _model->FrameCount(); ++f) - { - auto frame = _model->FrameByIndex(f); - if (outModel.Count(frame->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Frame with non-unique name [" + frame->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - auto frameId = - outModel.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - - if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) - { - // add edge from implicit model frame to frame - outModel.AddEdge({modelFrameId, frameId}, frame->RawPose()); - } - } + // add frame vertices + addVerticesToGraph(outModel, _model.frames, _model, errors); // add nested model vertices - for (uint64_t m = 0; m < _model->ModelCount(); ++m) - { - auto nestedModel = _model->ModelByIndex(m); - if (outModel.Count(nestedModel->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Nested model with non-unique name [" + nestedModel->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - - auto nestedErrors = buildPoseRelativeToGraph(outModel, nestedModel, false); - errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); - } - - // add nested interface model vertices - for (uint64_t m = 0; m < _model->InterfaceModelCount(); ++m) - { - auto ifaceModel = _model->InterfaceModelByIndex(m); - if (outModel.Count(ifaceModel->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Nested model with non-unique name [" + ifaceModel->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - - auto nestedErrors = buildPoseRelativeToGraph(outModel, ifaceModel); - errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); - } + addVerticesToGraph(outModel, _model.models, _model, errors); // now that all vertices have been added to the graph, // add the edges that reference other vertices - for (uint64_t l = 0; l < _model->LinkCount(); ++l) - { - auto link = _model->LinkByIndex(l); - - // check if we've already added a default edge - const std::string &relativeTo = link->PoseRelativeTo(); - if (relativeTo.empty()) - { - continue; - } - - auto linkId = outModel.VertexIdByName(link->Name()); - - // look for vertex in graph that matches relative_to value - if (outModel.Count(relativeTo) != 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by link with name[" + link->Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; - } - auto relativeToId = outModel.VertexIdByName(relativeTo); - if (link->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to link name[" + link->Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); - } - outModel.AddEdge({relativeToId, linkId}, link->RawPose()); - } - - for (uint64_t j = 0; j < _model->JointCount(); ++j) - { - auto joint = _model->JointByIndex(j); - - std::string relativeTo = joint->PoseRelativeTo(); - if (relativeTo.empty()) - { - // since nothing else was specified, use the joint's child frame - relativeTo = joint->ChildLinkName(); - } - - auto jointId = outModel.VertexIdByName(joint->Name()); - - // look for vertex in graph that matches relative_to value - if (outModel.Count(relativeTo) != 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by joint with name[" + joint->Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; - } - auto relativeToId = outModel.VertexIdByName(relativeTo); - if (joint->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to joint name[" + joint->Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); - } - outModel.AddEdge({relativeToId, jointId}, joint->RawPose()); - } - - for (uint64_t f = 0; f < _model->FrameCount(); ++f) - { - auto frame = _model->FrameByIndex(f); - - // check if we've already added a default edge - if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) - { - continue; - } - - auto frameId = outModel.VertexIdByName(frame->Name()); - std::string relativeTo; - std::string typeForErrorMsg; - ErrorCode errorCode; - if (!frame->PoseRelativeTo().empty()) - { - relativeTo = frame->PoseRelativeTo(); - typeForErrorMsg = "relative_to"; - errorCode = ErrorCode::POSE_RELATIVE_TO_INVALID; - } - else - { - relativeTo = frame->AttachedTo(); - typeForErrorMsg = "attached_to"; - errorCode = ErrorCode::FRAME_ATTACHED_TO_INVALID; - } - - // look for vertex in graph that matches relative_to value - if (outModel.Count(relativeTo) != 1) - { - errors.push_back({errorCode, - typeForErrorMsg + " name[" + relativeTo + - "] specified by frame with name[" + frame->Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; - } - auto relativeToId = outModel.VertexIdByName(relativeTo); - if (frame->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to frame name[" + frame->Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); - } - outModel.AddEdge({relativeToId, frameId}, frame->RawPose()); - } - - for (uint64_t m = 0; m < _model->ModelCount(); ++m) - { - auto nestedModel = _model->ModelByIndex(m); - - auto nestedModelId = outModel.VertexIdByName(nestedModel->Name()); - // if relative_to is empty, add edge from implicit model frame to - // nestedModel - auto relativeToId = modelFrameId; - - const std::string &relativeTo = nestedModel->PoseRelativeTo(); - if (!relativeTo.empty()) - { - // look for vertex in graph that matches relative_to value - if (outModel.Count(relativeTo) != 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by nested model with name[" + nestedModel->Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; - } - - relativeToId = outModel.VertexIdByName(relativeTo); - if (nestedModel->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to nested model name[" + nestedModel->Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); - } - } - - ignition::math::Pose3d resolvedModelPose = nestedModel->RawPose(); - sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( - nestedModel->RawPose(), nestedModel->PlacementFrameName(), - outModel.ChildModelScope(nestedModel->Name()), resolvedModelPose); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - - outModel.AddEdge({relativeToId, nestedModelId}, resolvedModelPose); - } - for (uint64_t m = 0; m < _model->InterfaceModelCount(); ++m) - { - auto ifaceModel = _model->InterfaceModelByIndex(m); - const auto *nestedInclude = _model->InterfaceModelNestedIncludeByIndex(m); - - auto nestedModelId = outModel.VertexIdByName(ifaceModel->Name()); - // if relative_to is empty, add edge from implicit model frame to - // nestedModel - auto relativeToId = modelFrameId; - - const std::string &relativeTo = - nestedInclude->IncludePoseRelativeTo().value_or(""); - if (!relativeTo.empty()) - { - // look for vertex in graph that matches relative_to value - if (outModel.Count(relativeTo) != 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by nested model with name[" + ifaceModel->Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; - } - - relativeToId = outModel.VertexIdByName(relativeTo); - if (ifaceModel->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to nested model name[" + ifaceModel->Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); - } - } + // add link edges + addEdgesToGraph(outModel, _model.links, _model, errors); - ignition::math::Pose3d resolvedModelPose = - nestedInclude->IncludeRawPose().value_or(ignition::math::Pose3d()); + // add joint edges + addEdgesToGraph(outModel, _model.joints, _model, errors); - sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( - nestedInclude->IncludeRawPose().value_or(ignition::math::Pose3d()), - nestedInclude->PlacementFrame().value_or(""), - outModel.ChildModelScope(ifaceModel->Name()), resolvedModelPose); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + // add frame edges + addEdgesToGraph(outModel, _model.frames, _model, errors); - outModel.AddEdge({relativeToId, nestedModelId}, resolvedModelPose); - } + // add nested model edges + addEdgesToGraph(outModel, _model.models, _model, errors); if (_isRoot) { @@ -1197,199 +1008,51 @@ Errors buildPoseRelativeToGraph( // sdf::resolvePoseRelativeToRoot. We will later update the edge after the // pose is calculated. auto rootToModel = outModel.AddEdge({rootId, modelId}, {}); - ignition::math::Pose3d resolvedModelPose = _model->RawPose(); + ignition::math::Pose3d resolvedModelPose = _model.rawPose; sdf::Errors resolveErrors = - resolveModelPoseWithPlacementFrame(_model->RawPose(), - _model->PlacementFrameName(), outModel, resolvedModelPose); + resolveModelPoseWithPlacementFrame(_model.rawPose, + _model.placementFrameName, outModel, resolvedModelPose); errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); outModel.UpdateEdge(rootToModel, resolvedModelPose); } return errors; } - ///////////////////////////////////////////////// -Errors buildPoseRelativeToGraph(ScopedGraph &_out, - InterfaceModelConstPtr _model) +Errors buildPoseRelativeToGraph( + ScopedGraph &_out, const Model *_model, bool _isRoot) { - Errors errors; - if (!_model) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid sdf::Model pointer."}); - return errors; - } - - const std::string scopeContextName = "__model__"; - // add the model frame vertex first - auto modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); - auto outModel = _out.AddScopeVertex(_model->Name(), scopeContextName, - scopeContextName, sdf::FrameType::MODEL); - auto modelFrameId = outModel.ScopeVertexId(); - - // Add an aliasing edge from the model vertex to the - // corresponding vertex in the PoseRelativeTo graph of the child model, - // i.e, to the ::__model__ vertex. - auto &edge = _out.AddEdge({modelId, modelFrameId}, {}); - // Set the edge weight to 0 to indicate that this is an aliasing edge. - edge.SetWeight(0); - - // add link vertices and default edge if relative_to is empty - for (const auto &link : _model->Links()) - { - if (outModel.Count(link.Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Link with non-unique name [" + link.Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - auto linkId = - outModel.AddVertex(link.Name(), sdf::FrameType::LINK).Id(); - - // relative_to is empty, so add edge from implicit model frame to link - outModel.AddEdge({modelFrameId, linkId}, link.PoseInModelFrame()); - } - - // add joint vertices - for (const auto &joint : _model->Joints()) - { - if (outModel.Count(joint.Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Joint with non-unique name [" + joint.Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - outModel.AddVertex(joint.Name(), sdf::FrameType::JOINT); - } - - // add frame vertices and default edge if both - // relative_to and attached_to are empty - for (const auto &frame : _model->Frames()) - { - if (outModel.Count(frame.Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Frame with non-unique name [" + frame.Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - auto frameId = outModel.AddVertex(frame.Name(), sdf::FrameType::FRAME).Id(); - if (frame.AttachedTo().empty()) - { - // add edge from implicit model frame to frame - outModel.AddEdge({modelFrameId, frameId}, frame.PoseInAttachedToFrame()); - } + return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::Model pointer."}}; } - - // add nested model vertices and default edge if relative_to is empty - for (const auto &nestedIfaceModel : _model->NestedModels()) + else if (!_model->Element()) { - if (outModel.Count(nestedIfaceModel->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Nested model with non-unique name [" + nestedIfaceModel->Name() + - "] detected in model with name [" + _model->Name() + - "]."}); - continue; - } - - auto nestedErrors = buildPoseRelativeToGraph(outModel, nestedIfaceModel); - errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); - auto nestedModelId = outModel.VertexIdByName(nestedIfaceModel->Name()); - outModel.AddEdge({modelFrameId, nestedModelId}, - nestedIfaceModel->ModelFramePoseInParentFrame()); + return Errors{ + {ErrorCode::ELEMENT_INVALID, "Invalid model element in sdf::Model."}}; } - for (const auto &joint : _model->Joints()) - { - const std::string &relativeTo = joint.ChildName(); - auto jointId = outModel.VertexIdByName(joint.Name()); - - // look for vertex in graph that matches relative_to value - if (outModel.Count(relativeTo) != 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by joint with name[" + joint.Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; - } - auto relativeToId = outModel.VertexIdByName(relativeTo); - if (joint.Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to joint name[" + joint.Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); - } - outModel.AddEdge({relativeToId, jointId}, joint.PoseInChildFrame()); - } + return wrapperBuildPoseRelativeToGraph(_out, ModelWrapper(*_model), _isRoot); +} - for (const auto &frame : _model->Frames()) +///////////////////////////////////////////////// +Errors buildPoseRelativeToGraph(ScopedGraph &_out, + const InterfaceModel *_model) +{ + if (!_model) { - if (frame.AttachedTo().empty()) - { - continue; - } - auto frameId = outModel.VertexIdByName(frame.Name()); - std::string relativeTo; - std::string typeForErrorMsg; - ErrorCode errorCode; - relativeTo = frame.AttachedTo(); - typeForErrorMsg = "attached_to"; - errorCode = ErrorCode::FRAME_ATTACHED_TO_INVALID; - - // look for vertex in graph that matches relative_to value - if (outModel.Count(relativeTo) != 1) - { - errors.push_back({errorCode, - "attached_to name[" + relativeTo + - "] specified by frame with name[" + frame.Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; - } - auto relativeToId = outModel.VertexIdByName(relativeTo); - if (frame.Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to frame name[" + frame.Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); - } - outModel.AddEdge({relativeToId, frameId}, frame.PoseInAttachedToFrame()); + return Errors{ + {ErrorCode::ELEMENT_INVALID, "Invalid sdf::InterfaceModel pointer."}}; } - - return errors; + return wrapperBuildPoseRelativeToGraph(_out, ModelWrapper(*_model), false); } ///////////////////////////////////////////////// -Errors buildPoseRelativeToGraph( - ScopedGraph &_out, const World *_world) +Errors wrapperBuildPoseRelativeToGraph( + ScopedGraph &_out, const WorldWrapper &_world) { Errors errors; - if (!_world) - { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid sdf::World pointer."}); - return errors; - } - else if (!_world->Element()) - { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid world element in sdf::World."}); - return errors; - } // add implicit world frame vertex first const std::string scopeContextName = "world"; @@ -1403,206 +1066,36 @@ Errors buildPoseRelativeToGraph( _out.AddEdge({rootId, worldFrameId}, {}); // add model vertices - for (uint64_t m = 0; m < _world->ModelCount(); ++m) - { - auto model = _world->ModelByIndex(m); - if (_out.Count(model->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Model with non-unique name [" + model->Name() + - "] detected in world with name [" + _world->Name() + - "]."}); - continue; - } - - auto modelErrors = buildPoseRelativeToGraph(_out , model, false); - errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); - } - - for (uint64_t m = 0; m < _world->InterfaceModelCount(); ++m) - { - auto ifaceModel = _world->InterfaceModelByIndex(m); - if (_out.Count(ifaceModel->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Model with non-unique name [" + ifaceModel->Name() + - "] detected in world with name [" + _world->Name() + - "]."}); - continue; - } + addVerticesToGraph(_out, _world.models, _world, errors); - auto modelErrors = - buildPoseRelativeToGraph(_out, ifaceModel); - errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); - } - - // add frame vertices and default edge if both - // relative_to and attached_to are empty - for (uint64_t f = 0; f < _world->FrameCount(); ++f) - { - auto frame = _world->FrameByIndex(f); - if (_out.Count(frame->Name()) > 0) - { - errors.push_back({ErrorCode::DUPLICATE_NAME, - "Frame with non-unique name [" + frame->Name() + - "] detected in world with name [" + _world->Name() + - "]."}); - continue; - } - auto frameId = - _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - - if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) - { - // add edge from implicit world frame to frame - _out.AddEdge({worldFrameId, frameId}, frame->RawPose()); - } - } + // add frame vertices + addVerticesToGraph(_out, _world.frames, _world, errors); // now that all vertices have been added to the graph, // add the edges that reference other vertices + // add model edges + addEdgesToGraph(_out, _world.models, _world, errors); - for (uint64_t m = 0; m < _world->ModelCount(); ++m) - { - auto model = _world->ModelByIndex(m); - - auto modelId = _out.VertexIdByName(model->Name()); - // if relative_to is empty, add edge from implicit model frame to - // world - auto relativeToId = worldFrameId; - - const std::string &relativeTo = model->PoseRelativeTo(); - if (!relativeTo.empty()) - { - // look for vertex in graph that matches relative_to value - if (_out.Count(relativeTo) != 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by model with name[" + model->Name() + - "] does not match a model or frame name " - "in world with name[" + _world->Name() + "]."}); - continue; - } - - relativeToId = _out.VertexIdByName(relativeTo); - if (model->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to model name[" + model->Name() + - "], causing a graph cycle " - "in world with name[" + _world->Name() + "]."}); - } - } - - ignition::math::Pose3d resolvedModelPose = model->RawPose(); - sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( - model->RawPose(), model->PlacementFrameName(), - _out.ChildModelScope(model->Name()), resolvedModelPose); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + // add frame edges + addEdgesToGraph(_out, _world.frames, _world, errors); - _out.AddEdge({relativeToId, modelId}, resolvedModelPose); - } + return errors; +} - for (uint64_t m = 0; m < _world->InterfaceModelCount(); ++m) +///////////////////////////////////////////////// +Errors buildPoseRelativeToGraph( + ScopedGraph &_out, const World *_world) +{ + if (!_world) { - auto ifaceModel = _world->InterfaceModelByIndex(m); - const auto *nestedInclude = _world->InterfaceModelNestedIncludeByIndex(m); - - auto modelId = _out.VertexIdByName(ifaceModel->Name()); - // if relative_to is empty, add edge from implicit model frame to - // world - auto relativeToId = worldFrameId; - - const std::string &relativeTo = - nestedInclude->IncludePoseRelativeTo().value_or(""); - if (!relativeTo.empty()) - { - // look for vertex in graph that matches relative_to value - if (_out.Count(relativeTo) != 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by interface model with name[" + ifaceModel->Name() + - "] does not match a model or frame name " - "in world with name[" + _world->Name() + "]."}); - continue; - } - - relativeToId = _out.VertexIdByName(relativeTo); - if (ifaceModel->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to interface model name[" + ifaceModel->Name() + - "], causing a graph cycle " - "in world with name[" + _world->Name() + "]."}); - } - } - - ignition::math::Pose3d resolvedModelPose = - nestedInclude->IncludeRawPose().value_or(ignition::math::Pose3d()); - - sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( - nestedInclude->IncludeRawPose().value_or(ignition::math::Pose3d()), - nestedInclude->PlacementFrame().value_or(""), - _out.ChildModelScope(ifaceModel->Name()), resolvedModelPose); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - - _out.AddEdge({relativeToId, modelId}, resolvedModelPose); + return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::World pointer."}}; } - - for (uint64_t f = 0; f < _world->FrameCount(); ++f) + else if (!_world->Element()) { - auto frame = _world->FrameByIndex(f); - - // check if we've already added a default edge - if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) - { - continue; - } - - auto frameId = _out.VertexIdByName(frame->Name()); - std::string relativeTo; - std::string typeForErrorMsg; - ErrorCode errorCode; - if (!frame->PoseRelativeTo().empty()) - { - relativeTo = frame->PoseRelativeTo(); - typeForErrorMsg = "relative_to"; - errorCode = ErrorCode::POSE_RELATIVE_TO_INVALID; - } - else - { - relativeTo = frame->AttachedTo(); - typeForErrorMsg = "attached_to"; - errorCode = ErrorCode::FRAME_ATTACHED_TO_INVALID; - } - - // look for vertex in graph that matches relative_to value - if (_out.Count(relativeTo) != 1) - { - errors.push_back({errorCode, - typeForErrorMsg + " name[" + relativeTo + - "] specified by frame with name[" + frame->Name() + - "] does not match a model or frame name " - "in world with name[" + _world->Name() + "]."}); - continue; - } - auto relativeToId = _out.VertexIdByName(relativeTo); - if (frame->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to frame name[" + frame->Name() + - "], causing a graph cycle " - "in world with name[" + _world->Name() + "]."}); - } - _out.AddEdge({relativeToId, frameId}, frame->RawPose()); + return Errors{ + {ErrorCode::ELEMENT_INVALID, "Invalid world element in sdf::World."}}; } - - return errors; + return wrapperBuildPoseRelativeToGraph(_out, WorldWrapper(*_world)); } ///////////////////////////////////////////////// diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 50e387d7d..249cecd3b 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -981,10 +981,10 @@ TEST(GraphCmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) << " 16 [label=\"F7 (16)\"];\n" << " 0 -> 1 [label=1];\n" << " 2 -> 3 [label=0];\n" - << " 3 -> 4 [label=1];\n" - << " 3 -> 5 [label=1];\n" << " 8 -> 9 [label=0];\n" << " 9 -> 10 [label=1];\n" + << " 3 -> 4 [label=1];\n" + << " 3 -> 5 [label=1];\n" << " 5 -> 6 [label=1];\n" << " 5 -> 7 [label=1];\n" << " 3 -> 8 [label=1];\n" @@ -1041,12 +1041,11 @@ TEST(GraphCmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) << " 29 [label=\"parent_model::M7::__model__ (29)\"];\n" << " 30 [label=\"parent_model::M7::L (30)\"];\n" << " 1 -> 2 [label=0];\n" - << " 2 -> 3 [label=1];\n" << " 4 -> 5 [label=0];\n" - << " 5 -> 6 [label=1];\n" - << " 5 -> 7 [label=1];\n" << " 10 -> 11 [label=0];\n" << " 11 -> 12 [label=1];\n" + << " 5 -> 6 [label=1];\n" + << " 5 -> 7 [label=1];\n" << " 7 -> 8 [label=1];\n" << " 7 -> 9 [label=1];\n" << " 5 -> 10 [label=1];\n" @@ -1062,6 +1061,7 @@ TEST(GraphCmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) << " 26 -> 27 [label=1];\n" << " 28 -> 29 [label=0];\n" << " 29 -> 30 [label=1];\n" + << " 2 -> 3 [label=1];\n" << " 2 -> 4 [label=1];\n" << " 4 -> 13 [label=1];\n" << " 6 -> 16 [label=1];\n" diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 90b15be93..ec2d112fc 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -489,7 +489,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(10u, errors.size()); + ASSERT_EQ(10u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -745,7 +745,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidAttachedTo) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(11u, errors.size()); + ASSERT_EQ(11u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( diff --git a/test/integration/interface_api.cc b/test/integration/interface_api.cc index badda1669..a250f5cef 100644 --- a/test/integration/interface_api.cc +++ b/test/integration/interface_api.cc @@ -269,7 +269,7 @@ TEST_F(InterfaceAPI, CustomParserPrecedence) this->config.RegisterCustomModelParser(createCustomParser(3)); sdf::Root root; sdf::Errors errors = root.LoadSdfString(testSdf, this->config); - EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; // Check that custom parsers are called in reverse order of registration. ASSERT_EQ(3u, customParserCallOrder.size()); // Parser 0 will not be visited because parser 1 will be successful. From 5e91938c45838714cf0a8a08255d97e2ffb36e25 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 21 Dec 2021 19:36:59 +0800 Subject: [PATCH 59/60] Support printing sdf poses in degrees and allow snapping to commonly used angles (#689) * Ruby option to print in_degrees or snap_to_degrees Signed-off-by: Aaron Chong * Basic PrintConfig added Signed-off-by: Aaron Chong * PrintConfig gets passed into printing implementations of Element and Param Signed-off-by: Aaron Chong * Adding basic test for print options Signed-off-by: Aaron Chong * Reverting to PrintConfig with basic API Signed-off-by: Aaron Chong * Moved creation of PrintConfig into ign functions Signed-off-by: Aaron Chong * Param value GetPoseAsString and tests Signed-off-by: Aaron Chong * Moved attribute painting to its own function, fixed test strings Signed-off-by: Aaron Chong * Added basic tests for pose rotation input as quaternions Signed-off-by: Aaron Chong * Using different flags for ign sdf -p, allow snapping to different values Signed-off-by: Aaron Chong * Disabling test on windows, fixing comment Signed-off-by: Aaron Chong * Remove stale function, fixed linting Signed-off-by: Aaron Chong * Adding tolerance as a argument, added tests Signed-off-by: Aaron Chong * Use 3 spaces when changing rotation formats or snapping to degrees Signed-off-by: Aaron Chong * Added check for tolerance larger than snapping interval Signed-off-by: Aaron Chong * Moving PrintAttributes to ElementPrivate to remain ABI stability Signed-off-by: Aaron Chong * Using true/false instead of 1/0 Signed-off-by: Aaron Chong * Remove use of SDF_ASSERT in GetAsString Signed-off-by: Aaron Chong * Added tests for //include/pose Signed-off-by: Aaron Chong * Adding parsing passing test for empty quat_xyzw pose Signed-off-by: Aaron Chong * Added check for default string values to be modified when rotation_format is defined Signed-off-by: Aaron Chong * Added tests Signed-off-by: Aaron Chong * Reparsing translates default value into string to be used if values have not been assigned to param Signed-off-by: Aaron Chong * Using StringFromValueImpl for getting strings from all ParamVariants Signed-off-by: Aaron Chong * Refactor pose string from value into its own function Signed-off-by: Aaron Chong * Fixing casting erroerror, added documentation and tests for tolerance < interval Signed-off-by: Aaron Chong * Correcting stale comments Signed-off-by: Aaron Chong * Fixing snapToInterval math, added more tests Signed-off-by: Aaron Chong * Removed unneeded visibility macro Signed-off-by: Aaron Chong * Adding return documentation and using const reference to variant instead of pointer Signed-off-by: Aaron Chong * Returning string directly, removing stale _config, reverting strValue to nullopt Signed-off-by: Aaron Chong * Remove use of assertions Signed-off-by: Aaron Chong * Suggested changes to #729 (#748) Signed-off-by: Addisu Z. Taddese * Using three space delimiter between position and rotation if attributes are set Signed-off-by: Aaron Chong * Added comment regarding use of default PrintConfig in Reparse Signed-off-by: Aaron Chong * Adding equality comparison for PrintConfig Signed-off-by: Aaron Chong * Removed stale include Signed-off-by: Aaron Chong * Uniied string and value parsing behavior, and modified necessary tests Signed-off-by: Aaron Chong * Overloaded function to maintain ABI stability Signed-off-by: Aaron Chong * Fixing missing space in test for exec command Signed-off-by: Aaron Chong * Adding comment regarding attributeExceptions Signed-off-by: Aaron Chong * Indenting help message, adding test for shuffling command flags Signed-off-by: Aaron Chong * Modifying cmd flag shuffling test to handling flags with arguments too Signed-off-by: Aaron Chong * Removed Get from PrintConfig getter functions Signed-off-by: Aaron Chong * Using std optional's converting constructor Signed-off-by: Aaron Chong * Modified snapToInterval implementation, added test Signed-off-by: Aaron Chong * Added bool type specific value parser, values are parsed using ParamStreamer by default Signed-off-by: Aaron Chong * Reverting all unnecessary changes made in sdf12 to old tests Signed-off-by: Aaron Chong * Added comparison for PreserveIncludes Signed-off-by: Aaron Chong * Check for 'type' attribute in unknown elements as well, in order to parse booleans into true/false, instead of 1/0 Signed-off-by: Aaron Chong * Only checking for pose related PrintConfig options for returning string instead of default PrintConfig Signed-off-by: Aaron Chong * Added comment regarding sanitizing -0 in test outputs Signed-off-by: Aaron Chong Co-authored-by: Addisu Z. Taddese --- include/sdf/Element.hh | 12 +- include/sdf/Param.hh | 33 +- include/sdf/PrintConfig.hh | 40 +- src/CMakeLists.txt | 2 +- src/Element.cc | 62 +- src/Param.cc | 171 ++++- src/Plugin_TEST.cc | 2 +- src/PrintConfig.cc | 76 ++- src/PrintConfig_TEST.cc | 75 +++ src/cmd/cmdsdformat.rb.in | 48 +- src/ign.cc | 17 +- src/ign_TEST.cc | 625 ++++++++++++++++++ src/parser.cc | 22 +- test/integration/default_elements.cc | 36 +- .../include_custom_model_expected_output.sdf | 10 +- ...de_custom_nested_model_expected_output.sdf | 52 +- test/integration/pose_1_9_sdf.cc | 26 +- test/integration/print_config.cc | 3 + test/sdf/includes_rotations_in_degrees.sdf | 12 + .../sdf/includes_rotations_in_quaternions.sdf | 14 + test/sdf/includes_rotations_in_radians.sdf | 12 + test/sdf/rotations_in_degrees.sdf | 8 + ...tations_in_degrees_high_snap_tolerance.sdf | 8 + test/sdf/rotations_in_quaternions.sdf | 10 + test/sdf/rotations_in_radians.sdf | 8 + .../sdf/rotations_in_unnormalized_degrees.sdf | 8 + .../sdf/rotations_in_unnormalized_radians.sdf | 8 + 27 files changed, 1262 insertions(+), 138 deletions(-) create mode 100644 test/sdf/includes_rotations_in_degrees.sdf create mode 100644 test/sdf/includes_rotations_in_quaternions.sdf create mode 100644 test/sdf/includes_rotations_in_radians.sdf create mode 100644 test/sdf/rotations_in_degrees.sdf create mode 100644 test/sdf/rotations_in_degrees_high_snap_tolerance.sdf create mode 100644 test/sdf/rotations_in_quaternions.sdf create mode 100644 test/sdf/rotations_in_radians.sdf create mode 100644 test/sdf/rotations_in_unnormalized_degrees.sdf create mode 100644 test/sdf/rotations_in_unnormalized_radians.sdf diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh index 99cc471c8..52ce5f7cb 100644 --- a/include/sdf/Element.hh +++ b/include/sdf/Element.hh @@ -199,7 +199,7 @@ namespace sdf /// \param[in] _prefix String value to prefix to the output. /// \param[in] _includeDefaultElements flag to include default elements. /// \param[in] _includeDefaultAttributes flag to include default attributes. - /// \param[in] _config Configuration for printing the values. + /// \param[in] _config Configuration for converting to string. /// \return The string representation. public: std::string ToString( const std::string &_prefix, @@ -589,7 +589,7 @@ namespace sdf /// \param[in] _includeDefaultElements flag to include default elements. /// \param[in] _includeDefaultAttributes flag to include default attributes. /// \param[in] _config Configuration for printing values. - /// \param[out] _out the std::ostreamstream to write output to. + /// \param[out] _out the std::ostringstream to write output to. private: void PrintValuesImpl(const std::string &_prefix, bool _includeDefaultElements, bool _includeDefaultAttributes, @@ -685,6 +685,14 @@ namespace sdf /// \brief XML path of this element. public: std::string xmlPath; + + /// \brief Generate the string (XML) for the attributes. + /// \param[in] _includeDefaultAttributes flag to include default attributes. + /// \param[in] _config Configuration for printing attributes. + /// \param[out] _out the std::ostringstream to write output to. + public: void PrintAttributes(bool _includeDefaultAttributes, + const PrintConfig &_config, + std::ostringstream &_out) const; }; /////////////////////////////////////////////// diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index d46204be3..827ba3f06 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -401,17 +401,34 @@ namespace sdf const std::string &_valueStr, ParamVariant &_valueToSet) const; - /// \brief Method used to get the string representation from a ParamVariant + /// \brief Method used to get the string representation from a ParamVariant, + /// or the string that was used to set it. /// \param[in] _config Print configuration for the string output /// \param[in] _typeName The data type of the value /// \param[in] _value The value - /// \param[in] _valueStr The string representation of the value - /// \return True if the string was successfully retrieved from the value, - /// false otherwise. - public: bool StringFromValueImpl(const PrintConfig &_config, - const std::string &_typeName, - const ParamVariant &_value, - std::string &_valueStr) const; + /// \param[out] _valueStr The output string. + /// \return True if the string was successfully retrieved, false otherwise. + public: bool StringFromValueImpl( + const PrintConfig &_config, + const std::string &_typeName, + const ParamVariant &_value, + std::string &_valueStr) const; + + /// \brief Method used to get the string representation from a ParamVariant, + /// or the string that was used to set it. + /// \param[in] _config Print configuration for the string output + /// \param[in] _typeName The data type of the value + /// \param[in] _value The value + /// \param[in] _orignalStr The original string that was used to set the + /// value. A nullopt can be passed in if it is not available. + /// \param[out] _valueStr The output string. + /// \return True if the string was successfully retrieved, false otherwise. + public: bool StringFromValueImpl( + const PrintConfig &_config, + const std::string &_typeName, + const ParamVariant &_value, + const std::optional &_originalStr, + std::string &_valueStr) const; /// \brief Data type to string mapping /// \return The type as a string, empty string if unknown type diff --git a/include/sdf/PrintConfig.hh b/include/sdf/PrintConfig.hh index 2e3b700de..8696339c3 100644 --- a/include/sdf/PrintConfig.hh +++ b/include/sdf/PrintConfig.hh @@ -17,6 +17,7 @@ #ifndef SDF_PRINTCONFIG_HH_ #define SDF_PRINTCONFIG_HH_ +#include #include #include "sdf/sdf_config.h" @@ -29,9 +30,41 @@ namespace sdf /// This class contains configuration options for printing elements. class SDFORMAT_VISIBLE PrintConfig { - /// \brief Default constructor. + /// \brief Default constructor. All options are set to false by default. public: PrintConfig(); + /// \brief Sets the option for printing pose rotations in degrees if true, + /// otherwise they will be printed as radians by default. + /// \param[in] _value Whether to print pose rotations in degrees. + public: void SetRotationInDegrees(bool _value); + + /// \brief Returns whether or not pose rotations should be printed in + /// degrees. + /// \return True if pose rotations are printed in degrees, false otherwise. + public: bool RotationInDegrees() const; + + /// \brief Sets the option for printing pose rotation in degrees as well as + /// snapping the rotation to the desired interval, with the provided + /// tolerance. + /// \param[in] _interval Degrees interval to snap to, this value must be + /// larger than 0, and less than or equal to 360. + /// \param[in] _tolerance Tolerance which snapping occurs, this value must + /// be larger than 0, less than 360, and less than the provided interval. + /// \return True, unless any of the provided values are not valid. + public: bool SetRotationSnapToDegrees(unsigned int _interval, + double _tolerance); + + /// \brief Returns the current degree value that pose rotations will snap to + /// when printed. + /// \return The assigned degrees interval value to snap to. If it has not + /// been assigned, a nullopt will be returned. + public: std::optional RotationSnapToDegrees() const; + + /// \brief Returns the tolerance for snapping degree values when printed. + /// \return The assigned tolerance value which allows snapping to happen. If + /// it has not been assigned, a nullopt will be returned. + public: std::optional RotationSnapTolerance() const; + /// \brief Set print config to preserve tags. /// \param[in] _preserve True to preserve tags. /// False to expand included model. @@ -42,6 +75,11 @@ namespace sdf /// False if they are to be expanded. public: bool PreserveIncludes() const; + /// \brief Return true if both PrintConfig objects contain the same values. + /// \param[in] _config PrintConfig to compare. + /// \return True if 'this' == _config. + public: bool operator==(const PrintConfig &_config) const; + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 597c6f5d8..fb25f7f4b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -136,8 +136,8 @@ if (BUILD_SDF_TEST) Pbr_TEST.cc Physics_TEST.cc Plugin_TEST.cc - PrintConfig_TEST.cc Plane_TEST.cc + PrintConfig_TEST.cc Root_TEST.cc Scene_TEST.cc SemanticPose_TEST.cc diff --git a/src/Element.cc b/src/Element.cc index 168403b8c..68f65e1c5 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -499,6 +499,7 @@ void Element::PrintDocLeftPane(std::string &_html, int _spacing, _html += "
\n"; } +///////////////////////////////////////////////// void Element::PrintValuesImpl(const std::string &_prefix, bool _includeDefaultElements, bool _includeDefaultAttributes, @@ -513,22 +514,7 @@ void Element::PrintValuesImpl(const std::string &_prefix, { _out << _prefix << "<" << this->dataPtr->name; - Param_V::const_iterator aiter; - for (aiter = this->dataPtr->attributes.begin(); - aiter != this->dataPtr->attributes.end(); ++aiter) - { - // Only print attribute values if they were set - // TODO(anyone): GetRequired is added here to support up-conversions where - // a new required attribute with a default value is added. We would have - // better separation of concerns if the conversion process set the - // required attributes with their default values. - if ((*aiter)->GetSet() || (*aiter)->GetRequired() || - _includeDefaultAttributes) - { - _out << " " << (*aiter)->GetKey() << "='" - << (*aiter)->GetAsString(_config) << "'"; - } - } + this->dataPtr->PrintAttributes(_includeDefaultAttributes, _config, _out); if (this->dataPtr->elements.size() > 0) { @@ -560,6 +546,50 @@ void Element::PrintValuesImpl(const std::string &_prefix, } } +///////////////////////////////////////////////// +void ElementPrivate::PrintAttributes(bool _includeDefaultAttributes, + const PrintConfig &_config, + std::ostringstream &_out) const +{ + // Attribute exceptions are used in the event of a non-default PrintConfig + // which modifies the Attributes of this Element that are printed out. The + // modifications to an Attribute by a PrintConfig will overwrite the original + // existing Attribute when this Element is printed. + std::set attributeExceptions; + if (this->name == "pose") + { + if (_config.RotationInDegrees() || _config.RotationSnapToDegrees()) + { + attributeExceptions.insert("degrees"); + _out << " " << "degrees='true'"; + + attributeExceptions.insert("rotation_format"); + _out << " " << "rotation_format='euler_rpy'"; + } + } + + Param_V::const_iterator aiter; + for (aiter = this->attributes.begin(); + aiter != this->attributes.end(); ++aiter) + { + // Only print attribute values if they were set + // TODO(anyone): GetRequired is added here to support up-conversions where + // a new required attribute with a default value is added. We would have + // better separation of concerns if the conversion process set the + // required attributes with their default values. + if ((*aiter)->GetSet() || (*aiter)->GetRequired() || + _includeDefaultAttributes) + { + const std::string key = (*aiter)->GetKey(); + const auto it = attributeExceptions.find(key); + if (it == attributeExceptions.end()) + { + _out << " " << key << "='" << (*aiter)->GetAsString(_config) << "'"; + } + } + } +} + ///////////////////////////////////////////////// void Element::PrintValues(std::string _prefix, const PrintConfig &_config) const { diff --git a/src/Param.cc b/src/Param.cc index 4c6ee2db1..0d370bd3c 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -308,13 +308,15 @@ void Param::Update() ////////////////////////////////////////////////// std::string Param::GetAsString(const PrintConfig &_config) const { - if (this->dataPtr->strValue.has_value() && !this->dataPtr->strValue->empty()) - { - return this->dataPtr->strValue.value(); - } - else if(!this->dataPtr->strValue.has_value()) + std::string valueStr; + if (this->GetSet() && + this->dataPtr->StringFromValueImpl(_config, + this->dataPtr->typeName, + this->dataPtr->value, + this->dataPtr->strValue, + valueStr)) { - return this->dataPtr->defaultStrValue; + return valueStr; } return this->GetDefaultAsString(_config); @@ -324,10 +326,12 @@ std::string Param::GetAsString(const PrintConfig &_config) const std::string Param::GetDefaultAsString(const PrintConfig &_config) const { std::string defaultStr; - if (this->dataPtr->StringFromValueImpl(_config, - this->dataPtr->typeName, - this->dataPtr->defaultValue, - defaultStr)) + if (this->dataPtr->StringFromValueImpl( + _config, + this->dataPtr->typeName, + this->dataPtr->defaultValue, + this->dataPtr->defaultStrValue, + defaultStr)) { return defaultStr; } @@ -785,13 +789,22 @@ bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, return true; } +////////////////////////////////////////////////// +/// \brief Helper function for StringFromValueImpl for pose. +/// \param[in] _config Printing configuration for the output string. +/// \param[in] _parentAttributes Parent Element Attributes. +/// \param[in] _value The variant value of this pose. +/// \param[in] _originalStr The original string used to set this pose value. +/// \param[out] _valueStr The pose as a string. +/// \return True if the string was successfully retrieved from the pose, false +/// otherwise. ///////////////////////////////////////////////// bool PoseStringFromValue(const PrintConfig &_config, const Param_V &_parentAttributes, const ParamPrivate::ParamVariant &_value, + const std::optional &_originalStr, std::string &_valueStr) { - (void)_config; StringStreamClassicLocale ss; const ignition::math::Pose3d *pose = @@ -802,20 +815,6 @@ bool PoseStringFromValue(const PrintConfig &_config, return false; } - auto sanitizeZero = [](double _number) - { - StringStreamClassicLocale stream; - if (std::fpclassify(_number) == FP_ZERO) - { - stream << 0; - } - else - { - stream << _number; - } - return stream.str(); - }; - const bool defaultInDegrees = false; bool inDegrees = defaultInDegrees; @@ -828,6 +827,10 @@ bool PoseStringFromValue(const PrintConfig &_config, const std::string threeSpacedDelimiter = " "; std::string posRotDelimiter = defaultPosRotDelimiter; + const bool defaultSnapDegreesToInterval = false; + bool snapDegreesToInterval = defaultSnapDegreesToInterval; + + // Checking parent Element Attributes for desired pose representations. for (const auto &p : _parentAttributes) { const std::string key = p->GetKey(); @@ -854,6 +857,39 @@ bool PoseStringFromValue(const PrintConfig &_config, } } + // Checking PrintConfig for desired pose representations. This overrides + // any parent Element Attributes. + if (_config.RotationInDegrees()) + { + inDegrees = true; + rotationFormat = "euler_rpy"; + posRotDelimiter = threeSpacedDelimiter; + } + if (_config.RotationSnapToDegrees().has_value() && + _config.RotationSnapTolerance().has_value()) + { + inDegrees = true; + rotationFormat = "euler_rpy"; + snapDegreesToInterval = true; + posRotDelimiter = threeSpacedDelimiter; + } + + // Helper function that sanitizes zero values like '-0' + auto sanitizeZero = [](double _number) + { + StringStreamClassicLocale stream; + if (std::fpclassify(_number) == FP_ZERO) + { + stream << 0; + } + else + { + stream << _number; + } + return stream.str(); + }; + + // Returning pose string representations based on desired configurations. if (rotationFormat == "quat_xyzw" && inDegrees) { sdferr << "Invalid pose with //pose[@degrees='true'] and " @@ -870,6 +906,36 @@ bool PoseStringFromValue(const PrintConfig &_config, _valueStr = ss.str(); return true; } + else if (rotationFormat == "euler_rpy" && inDegrees && snapDegreesToInterval) + { + // Helper function that returns a snapped value if it is within the + // tolerance of multiples of interval, otherwise the orginal value is + // returned. + auto snapToInterval = + [](double _val, unsigned int _interval, double _tolerance) + { + double closestQuotient = std::round(_val / _interval); + double distance = std::abs(_val - closestQuotient * _interval); + if (distance < _tolerance) + { + return _interval * closestQuotient; + } + return _val; + }; + + const unsigned int interval = _config.RotationSnapToDegrees().value(); + const double tolerance = _config.RotationSnapTolerance().value(); + + ss << pose->Pos() << posRotDelimiter + << sanitizeZero(snapToInterval( + IGN_RTOD(pose->Rot().Roll()), interval, tolerance)) << " " + << sanitizeZero(snapToInterval( + IGN_RTOD(pose->Rot().Pitch()), interval, tolerance)) << " " + << sanitizeZero(snapToInterval( + IGN_RTOD(pose->Rot().Yaw()), interval, tolerance)); + _valueStr = ss.str(); + return true; + } else if (rotationFormat == "euler_rpy" && inDegrees) { ss << pose->Pos() << posRotDelimiter @@ -880,6 +946,17 @@ bool PoseStringFromValue(const PrintConfig &_config, return true; } + // If no modification to the value is needed, the original string is returned. + if (!_config.RotationInDegrees() && + !_config.RotationSnapToDegrees().has_value() && + !_config.RotationSnapTolerance().has_value() && + _originalStr.has_value() && + !_originalStr->empty()) + { + _valueStr = _originalStr.value(); + return true; + } + ss << pose->Pos() << posRotDelimiter << sanitizeZero(pose->Rot().Roll()) << " " << sanitizeZero(pose->Rot().Pitch()) << " " @@ -889,12 +966,42 @@ bool PoseStringFromValue(const PrintConfig &_config, } ///////////////////////////////////////////////// -bool ParamPrivate::StringFromValueImpl(const PrintConfig &_config, - const std::string &_typeName, - const ParamVariant &_value, - std::string &_valueStr) const +bool ParamPrivate::StringFromValueImpl( + const PrintConfig &_config, + const std::string &_typeName, + const ParamVariant &_value, + std::string &_valueStr) const +{ + return this->StringFromValueImpl( + _config, + _typeName, + _value, + std::nullopt, + _valueStr); +} + +///////////////////////////////////////////////// +bool ParamPrivate::StringFromValueImpl( + const PrintConfig &_config, + const std::string &_typeName, + const ParamVariant &_value, + const std::optional &_originalStr, + std::string &_valueStr) const { - if (_typeName == "ignition::math::Pose3d" || + // This will be handled in a type specific manner + if (_typeName == "bool") + { + const bool *val = std::get_if(&_value); + if (!val) + { + sdferr << "Unable to get bool value from variant.\n"; + return false; + } + + _valueStr = *val ? "true" : "false"; + return true; + } + else if (_typeName == "ignition::math::Pose3d" || _typeName == "pose" || _typeName == "Pose") { @@ -902,9 +1009,9 @@ bool ParamPrivate::StringFromValueImpl(const PrintConfig &_config, if (!this->ignoreParentAttributes && p) { return PoseStringFromValue( - _config, p->GetAttributes(), _value, _valueStr); + _config, p->GetAttributes(), _value, _originalStr, _valueStr); } - return PoseStringFromValue(_config, {}, _value, _valueStr); + return PoseStringFromValue(_config, {}, _value, _originalStr, _valueStr); } StringStreamClassicLocale ss; diff --git a/src/Plugin_TEST.cc b/src/Plugin_TEST.cc index 00d5b6245..35f6aee0d 100644 --- a/src/Plugin_TEST.cc +++ b/src/Plugin_TEST.cc @@ -201,7 +201,7 @@ TEST(DOMPlugin, LoadWithChildren) pluginStr + ""; sdf::ElementPtr elem(new sdf::Element); sdf::initFile("plugin.sdf", elem); - sdf::readString(pluginStrWithSdf, elem); + ASSERT_TRUE(sdf::readString(pluginStrWithSdf, elem)); sdf::Plugin plugin; sdf::Errors errors; diff --git a/src/PrintConfig.cc b/src/PrintConfig.cc index 3770f5007..a36c441b5 100644 --- a/src/PrintConfig.cc +++ b/src/PrintConfig.cc @@ -16,14 +16,24 @@ */ #include "sdf/PrintConfig.hh" +#include "sdf/Console.hh" + +using namespace sdf; -namespace sdf -{ -inline namespace SDF_VERSION_NAMESPACE -{ ///////////////////////////////////////////////// class PrintConfig::Implementation { + /// \brief True if rotation in poses are to be printed in degrees. + public: bool rotationInDegrees = false; + + /// \brief The interval in degrees, which rotation in poses shall snap to, + /// if they are within the tolerance value of rotationSnapTolerance. + public: std::optional rotationSnapToDegrees = std::nullopt; + + /// \brief The tolerance which is used to determine whether snapping of + /// rotation in poses happen. + public: std::optional rotationSnapTolerance = std::nullopt; + /// \brief True to preserve tags, false to expand. public: bool preserveIncludes = false; }; @@ -34,6 +44,18 @@ PrintConfig::PrintConfig() { } +///////////////////////////////////////////////// +void PrintConfig::SetRotationInDegrees(bool _value) +{ + this->dataPtr->rotationInDegrees = _value; +} + +///////////////////////////////////////////////// +bool PrintConfig::RotationInDegrees() const +{ + return this->dataPtr->rotationInDegrees; +} + ///////////////////////////////////////////////// void PrintConfig::SetPreserveIncludes(bool _preserve) { @@ -46,5 +68,51 @@ bool PrintConfig::PreserveIncludes() const return this->dataPtr->preserveIncludes; } +///////////////////////////////////////////////// +bool PrintConfig::SetRotationSnapToDegrees(unsigned int _interval, + double _tolerance) +{ + if (_interval == 0 || _interval > 360) + { + sdferr << "Interval value to snap to must be larger than 0, and less than " + << "or equal to 360.\n"; + return false; + } + + if (_tolerance <= 0 || _tolerance > 360 || + _tolerance >= static_cast(_interval)) + { + sdferr << "Tolerance must be larger than 0, less than or equal to " + << "360, and less than the provided interval.\n"; + return false; + } + + this->dataPtr->rotationSnapToDegrees = _interval; + this->dataPtr->rotationSnapTolerance = _tolerance; + return true; +} + +///////////////////////////////////////////////// +std::optional PrintConfig::RotationSnapToDegrees() const +{ + return this->dataPtr->rotationSnapToDegrees; } + +///////////////////////////////////////////////// +std::optional PrintConfig::RotationSnapTolerance() const +{ + return this->dataPtr->rotationSnapTolerance; +} + +///////////////////////////////////////////////// +bool PrintConfig::operator==(const PrintConfig &_config) const +{ + if (this->RotationInDegrees() == _config.RotationInDegrees() && + this->RotationSnapToDegrees() == _config.RotationSnapToDegrees() && + this->RotationSnapTolerance() == _config.RotationSnapTolerance() && + this->PreserveIncludes() == _config.PreserveIncludes()) + { + return true; + } + return false; } diff --git a/src/PrintConfig_TEST.cc b/src/PrintConfig_TEST.cc index ff473bf5d..9ac523c9e 100644 --- a/src/PrintConfig_TEST.cc +++ b/src/PrintConfig_TEST.cc @@ -20,6 +20,81 @@ #include "sdf/PrintConfig.hh" #include "test_config.h" +///////////////////////////////////////////////// +TEST(PrintConfig, Construction) +{ + sdf::PrintConfig config; + EXPECT_FALSE(config.RotationInDegrees()); + EXPECT_FALSE(config.RotationSnapToDegrees()); + EXPECT_FALSE(config.PreserveIncludes()); +} + +///////////////////////////////////////////////// +TEST(PrintConfig, RotationInDegrees) +{ + sdf::PrintConfig config; + EXPECT_FALSE(config.RotationInDegrees()); + + config.SetRotationInDegrees(true); + EXPECT_TRUE(config.RotationInDegrees()); + + config.SetRotationInDegrees(false); + EXPECT_FALSE(config.RotationInDegrees()); +} + +///////////////////////////////////////////////// +TEST(PrintConfig, RotationSnapToDegrees) +{ + sdf::PrintConfig config; + EXPECT_FALSE(config.RotationSnapToDegrees().has_value()); + EXPECT_FALSE(config.RotationSnapTolerance().has_value()); + + EXPECT_TRUE(config.SetRotationSnapToDegrees(5, 0.01)); + ASSERT_TRUE(config.RotationSnapToDegrees().has_value()); + EXPECT_EQ(5u, config.RotationSnapToDegrees().value()); + ASSERT_TRUE(config.RotationSnapTolerance().has_value()); + EXPECT_DOUBLE_EQ(0.01, config.RotationSnapTolerance().value()); + + EXPECT_FALSE(config.SetRotationSnapToDegrees(0, 0.01)); + EXPECT_FALSE(config.SetRotationSnapToDegrees(360 + 1, 0.01)); + EXPECT_FALSE(config.SetRotationSnapToDegrees(5, -1e6)); + EXPECT_FALSE(config.SetRotationSnapToDegrees(5, 360 + 1e-6)); + + EXPECT_FALSE(config.SetRotationSnapToDegrees(5, 5 + 1e-6)); + EXPECT_TRUE(config.SetRotationSnapToDegrees(5, 5 - 1e-6)); + ASSERT_TRUE(config.RotationSnapToDegrees().has_value()); + EXPECT_EQ(5u, config.RotationSnapToDegrees().value()); + ASSERT_TRUE(config.RotationSnapTolerance().has_value()); + EXPECT_DOUBLE_EQ(5 - 1e-6, config.RotationSnapTolerance().value()); +} + +///////////////////////////////////////////////// +TEST(PrintConfig, Compare) +{ + sdf::PrintConfig first; + sdf::PrintConfig second; + EXPECT_TRUE(first == second); + EXPECT_TRUE(second == first); + + first.SetRotationInDegrees(true); + EXPECT_TRUE(first.RotationInDegrees()); + EXPECT_FALSE(second.RotationInDegrees()); + EXPECT_FALSE(first == second); + EXPECT_FALSE(second == first); + + second.SetRotationInDegrees(true); + EXPECT_TRUE(first == second); + EXPECT_TRUE(second == first); + + EXPECT_TRUE(first.SetRotationSnapToDegrees(5, 0.01)); + EXPECT_FALSE(first == second); + EXPECT_FALSE(second == first); + + EXPECT_TRUE(second.SetRotationSnapToDegrees(5, 0.01)); + EXPECT_TRUE(first == second); + EXPECT_TRUE(second == first); +} + ///////////////////////////////////////////////// TEST(PrintConfig, PreserveIncludes) { diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 9d4d757c4..5de7a65b3 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -31,9 +31,9 @@ require 'optparse' LIBRARY_NAME = '@library_location@' LIBRARY_VERSION = '@SDF_VERSION_FULL@' COMMON_OPTIONS = - " -h [ --help ] Print this help message.\n"\ - " --force-version Use a specific library version.\n"\ - ' --versions Show the available versions.' + " -h [ --help ] Print this help message.\n"\ + " --force-version Use a specific library version.\n"\ + ' --versions Show the available versions.' COMMANDS = { 'sdf' => "Utilities for SDF files.\n\n"\ " ign sdf [options]\n\n"\ @@ -44,6 +44,12 @@ COMMANDS = { 'sdf' => " use only and the output may change without any promise of stability)\n" + " -p [ --print ] arg Print converted arg.\n" + " -i [ --preserve-includes ] Preserve included tags when printing converted arg (does not preserve merge-includes).\n" + + " --degrees Pose rotation angles are printed in degrees.\n" + + " --snap-to-degrees arg Snap pose rotation angles to this specified interval in degrees. This value must be\n" + + " larger than 0, less than or equal to 360, and larger than the defined snap tolerance.\n" + + " --snap-tolerance arg Used in conjunction with --snap-to-degrees, specifies the tolerance at which snapping\n" + + " occurs. This value must be larger than 0, less than 360, and less than the defined\n" + + " degrees value to snap to. If unspecified, its default value is 0.01.\n" + COMMON_OPTIONS } @@ -57,6 +63,8 @@ class Cmd # def parse(args) options = {} + options['degrees'] = 0 + options['snap_tolerance'] = 0.01 usage = COMMANDS[args[0]] @@ -82,6 +90,25 @@ class Cmd opts.on('-i', '--preserve-includes', 'Preserve included tags when printing converted arg (does not preserve merge-includes)') do options['preserve_includes'] = 1 end + opts.on('--degrees', 'Printed pose rotations are will be in degrees') do |degrees| + options['degrees'] = 1 + end + opts.on('--snap-to-degrees arg', Integer, + 'Printed rotations are snapped to specified degree intervals') do |arg| + if arg == 0 || arg > 360 + puts "Degree interval to snap to must be more than 0, and less than or equal to 360." + exit(-1) + end + options['snap_to_degrees'] = arg + end + opts.on('--snap-tolerance arg', Float, + 'Printed rotations are snapped if they are within this specified tolerance') do |arg| + if arg < 0 || arg > 360 + puts "Rotation snapping tolerance must be more than 0, and less than 360." + exit(-1) + end + options['snap_tolerance'] = arg + end opts.on('-g arg', '--graph type', String, 'Print PoseRelativeTo or FrameAttachedTo graph') do |graph_type| options['graph'] = {:type => graph_type} @@ -178,13 +205,22 @@ class Cmd Importer.extern 'int cmdDescribe(const char *)' exit(Importer.cmdDescribe(options['describe'])) elsif options.key?('print') + snap_to_degrees = 0 if options['preserve_includes'] Importer.extern 'int cmdPrintPreserveIncludes(const char *)' exit(Importer.cmdPrintPreserveIncludes(File.expand_path(options['print']))) - else - Importer.extern 'int cmdPrint(const char *)' - exit(Importer.cmdPrint(File.expand_path(options['print']))) + elsif options.key?('snap_to_degrees') + if options['snap_to_degrees'] < options['snap_tolerance'] + puts "Rotation snapping tolerance must be larger than the snapping tolerance." + exit(-1) + end + snap_to_degrees = options['snap_to_degrees'] end + Importer.extern 'int cmdPrint(const char *, int in_degrees, int snap_to_degrees, float snap_tolerance)' + exit(Importer.cmdPrint(File.expand_path(options['print']), + options['degrees'], + snap_to_degrees, + options['snap_tolerance'])) elsif options.key?('graph') Importer.extern 'int cmdGraph(const char *, const char *)' exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1]))) diff --git a/src/ign.cc b/src/ign.cc index 16cdb0d24..23f9625e8 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -18,12 +18,14 @@ #include #include #include +#include #include #include "sdf/sdf_config.h" #include "sdf/Filesystem.hh" #include "sdf/Root.hh" #include "sdf/parser.hh" +#include "sdf/PrintConfig.hh" #include "sdf/system_util.hh" #include "FrameSemantics.hh" @@ -131,7 +133,8 @@ extern "C" SDFORMAT_VISIBLE int cmdDescribe(const char *_version) } ////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path) +extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, + int inDegrees, int snapToDegrees, float snapTolerance) { if (!sdf::filesystem::exists(_path)) { @@ -153,8 +156,18 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path) return -1; } - sdf->PrintValues(); + sdf::PrintConfig config; + if (inDegrees!= 0) + { + config.SetRotationInDegrees(true); + } + if (snapToDegrees > 0) + { + config.SetRotationSnapToDegrees(static_cast(snapToDegrees), + static_cast(snapTolerance)); + } + sdf->PrintValues(config); return 0; } diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 249cecd3b..d93172742 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -948,6 +948,631 @@ TEST(print, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } } +////////////////////////////////////////////////// +static bool contains(const std::string &_a, const std::string &_b) +{ + return _a.find(_b) != std::string::npos; +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_degrees.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 30.009 44.991 -60.009"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_radians.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 0.523756 0.785241 -1.04735"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.0087"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.0087"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const auto path = sdf::testing::TestFile( + "sdf", "rotations_in_quaternions.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 0.391948 0.200425 -0.532046 0.723279"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + // Set SDF_PATH so that included models can be found + sdf::testing::setenv( + "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); + const std::string path = + sdf::testing::TestFile("sdf", "includes_rotations_in_degrees.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 30.009 44.991 -60.009"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + // Set SDF_PATH so that included models can be found + sdf::testing::setenv( + "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); + const std::string path = + sdf::testing::TestFile("sdf", "includes_rotations_in_radians.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 0.523756 0.785241 -1.04735"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.0087"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.0087"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_includes_rotations_in_quaternions, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + // Set SDF_PATH so that included models can be found + sdf::testing::setenv( + "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); + const auto path = sdf::testing::TestFile( + "sdf", "includes_rotations_in_quaternions.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 0.391948 0.200425 -0.532046 0.723279"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_unnormalized_degrees, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_unnormalized_degrees.sdf"); + + // Default printing + // Unnormalized degree values cannot be returned as is, as its string is + // returned by parsing the pose value, whenever a parent Element Attribute, + // or PrintConfig is used. + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 30.009 44.991 -60.009"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.991 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.991 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_rotations_in_unnormalized_radians, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); + + // Default printing + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "1 2 3 -5.75943 -11.78112 5.23583"); + + // Printing with in_degrees + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60.009"); + + // Printing with snap_to_degrees 5 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // Printing with snap_to_degrees 2 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 44.9915 -60"); + + // Printing with snap_to_degrees 20 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.008 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60.009"); + + // Printing with snap_to_degrees 5, snap_tolerance 0.01 + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.01 " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(shuffled_cmd_flags, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = + sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); + + // -p PATH --degrees + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60.009"); + + // --degrees -p PATH + output = custom_exec_str( + IgnCommand() + " sdf --degrees -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30.009 44.9915 -60.009"); + + // -p PATH --snap-to-degrees ARG + output = custom_exec_str( + IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // -p --snap-to-degrees ARG PATH + output = custom_exec_str( + IgnCommand() + " sdf -p --snap-to-degrees 5 " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); + + // --snap-to-degrees ARG -p PATH + output = custom_exec_str( + IgnCommand() + " sdf --snap-to-degrees 5 -p " + path + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 45 -60"); +} + +///////////////////////////////////////////////// +TEST(print_snap_to_degrees_tolerance_too_high, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const std::string path = sdf::testing::TestFile( + "sdf", + "rotations_in_degrees_high_snap_tolerance.sdf"); + + std::string output = custom_exec_str( + IgnCommand() + " sdf -p " + path + + " --snap-to-degrees 5 " + " --snap-tolerance 4" + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(contains, output, + "" + "1 2 3 30 50 60"); +} + ///////////////////////////////////////////////// TEST(GraphCmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) { diff --git a/src/parser.cc b/src/parser.cc index 0c45332b0..f4dc5758d 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1942,19 +1942,27 @@ void copyChildren(ElementPtr _sdf, ElementPtr element(new Element); element->SetParent(_sdf); element->SetName(elem_name); - if (elemXml->GetText() != nullptr) - { - element->AddValue("string", elemXml->GetText(), "1"); - } - + std::optional typeName = std::nullopt; for (const tinyxml2::XMLAttribute *attribute = elemXml->FirstAttribute(); attribute; attribute = attribute->Next()) { - element->AddAttribute(attribute->Name(), "string", "", 1, ""); - element->GetAttribute(attribute->Name())->SetFromString( + const std::string attributeName(attribute->Name()); + if (attributeName == "type") + typeName = attribute->Value(); + + element->AddAttribute(attributeName, "string", "", 1, ""); + element->GetAttribute(attributeName)->SetFromString( attribute->Value()); } + if (elemXml->GetText() != nullptr) + { + if (typeName.has_value()) + element->AddValue(typeName.value(), elemXml->GetText(), true); + else + element->AddValue("string", elemXml->GetText(), true); + } + copyChildren(element, elemXml, _onlyUnknown); _sdf->InsertElement(element); } diff --git a/test/integration/default_elements.cc b/test/integration/default_elements.cc index fb8434db8..40767e6e2 100644 --- a/test/integration/default_elements.cc +++ b/test/integration/default_elements.cc @@ -187,27 +187,27 @@ TEST(ExplicitlySetInFile, ToString) << "\n" << " \n" << " \n" - << " 1.0\n" + << " 1\n" << " 0 0 0\n" << " \n" << " \n" << " EARTH_WGS84\n" - << " 0.0\n" - << " 0.0\n" - << " 0.0\n" - << " 0.0\n" + << " 0\n" + << " 0\n" + << " 0\n" + << " 0\n" << " \n" << " 0 0 -9.8\n" - << " 5.5645e-6 22.8758e-6 -42.3884e-6\n" + << " 6e-06 2.3e-05 -4.2e-05\n" << " \n" << " \n" << " 0.001\n" - << " 1.0\n" + << " 1\n" << " 1000\n" << " \n" << " \n" - << " 0.4 0.4 0.4 1.0\n" - << " .7 .7 .7 1\n" + << " 0.4 0.4 0.4 1\n" + << " 0.7 0.7 0.7 1\n" << " true\n" << " \n" << " \n" @@ -221,27 +221,27 @@ TEST(ExplicitlySetInFile, ToString) << "\n" << " \n" << " \n" - << " 1.0\n" + << " 1\n" << " 0 0 0\n" << " \n" << " \n" << " EARTH_WGS84\n" - << " 0.0\n" - << " 0.0\n" - << " 0.0\n" - << " 0.0\n" + << " 0\n" + << " 0\n" + << " 0\n" + << " 0\n" << " \n" << " 0 0 -9.8\n" - << " 5.5645e-6 22.8758e-6 -42.3884e-6\n" + << " 6e-06 2.3e-05 -4.2e-05\n" << " \n" << " \n" << " 0.001\n" - << " 1.0\n" + << " 1\n" << " 1000\n" << " \n" << " \n" - << " 0.4 0.4 0.4 1.0\n" - << " .7 .7 .7 1\n" + << " 0.4 0.4 0.4 1\n" + << " 0.7 0.7 0.7 1\n" << " true\n" << " \n" << " \n" diff --git a/test/integration/include_custom_model_expected_output.sdf b/test/integration/include_custom_model_expected_output.sdf index e4584cd98..f554228c8 100644 --- a/test/integration/include_custom_model_expected_output.sdf +++ b/test/integration/include_custom_model_expected_output.sdf @@ -211,7 +211,7 @@ - 1.0469999999999999 + 1.047 1280 @@ -457,16 +457,16 @@ 0 0 -9.8 - 5.5645e-6 22.8758e-6 -42.3884e-6 + 6e-06 2.3e-05 -4.2e-05 0.001 - 1.0 + 1 1000 - 0.4 0.4 0.4 1.0 - .7 .7 .7 1 + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 true diff --git a/test/integration/include_custom_nested_model_expected_output.sdf b/test/integration/include_custom_nested_model_expected_output.sdf index 959a6b2cc..e7b3115f2 100644 --- a/test/integration/include_custom_nested_model_expected_output.sdf +++ b/test/integration/include_custom_nested_model_expected_output.sdf @@ -11,7 +11,7 @@ 0.126164 0 0 - 0.416519 + 0.41651899999999997 0 0.481014 @@ -80,13 +80,13 @@ -1.06 0 0 0 0 3.14 - 1.047 + 1.0469999999999999 320 240 - 0.1 + 0.10000000000000001 100 @@ -158,13 +158,13 @@ - 1.047 + 1.0469999999999999 320 240 - 0.1 + 0.10000000000000001 100 @@ -181,7 +181,7 @@ -0.2 0 0.3 0 0 3.14 - 0.05 + 0.050000000000000003 @@ -203,10 +203,10 @@ 2 - 0.145833 + 0.14583299999999999 0 0 - 0.145833 + 0.14583299999999999 0 0.125 @@ -214,7 +214,7 @@ - 0.3 + 0.29999999999999999 @@ -226,7 +226,7 @@ - 0.3 + 0.29999999999999999 @@ -239,10 +239,10 @@ 2 - 0.145833 + 0.14583299999999999 0 0 - 0.145833 + 0.14583299999999999 0 0.125 @@ -250,7 +250,7 @@ - 0.3 + 0.29999999999999999 @@ -262,7 +262,7 @@ - 0.3 + 0.29999999999999999 @@ -275,10 +275,10 @@ 2 - 0.145833 + 0.14583299999999999 0 0 - 0.145833 + 0.14583299999999999 0 0.125 @@ -286,7 +286,7 @@ - 0.3 + 0.29999999999999999 @@ -298,7 +298,7 @@ - 0.3 + 0.29999999999999999 @@ -311,8 +311,8 @@ 0 0 1 - -1.79769e+308 - 1.79769e+308 + -1.7976900000000001e+308 + 1.7976900000000001e+308 @@ -322,8 +322,8 @@ 0 0 1 - -1.79769e+308 - 1.79769e+308 + -1.7976900000000001e+308 + 1.7976900000000001e+308 @@ -334,16 +334,16 @@ 0 0 -9.8 - 5.5645e-6 22.8758e-6 -42.3884e-6 + 6e-06 2.3e-05 -4.2e-05 0.001 - 1.0 + 1 1000 - 0.4 0.4 0.4 1.0 - .7 .7 .7 1 + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 true diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc index c70a98037..492ff2cdc 100644 --- a/test/integration/pose_1_9_sdf.cc +++ b/test/integration/pose_1_9_sdf.cc @@ -788,7 +788,7 @@ TEST(Pose1_9, ToStringWithDegreesFalse) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='0'"); + EXPECT_PRED2(contains, elemStr, "degrees='false'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -811,7 +811,7 @@ TEST(Pose1_9, ToStringWithDegreesTrue) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='1'"); + EXPECT_PRED2(contains, elemStr, "degrees='true'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -863,7 +863,7 @@ TEST(Pose1_9, ToStringWithEulerRPYDegreesTrue) EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='1'"); + EXPECT_PRED2(contains, elemStr, "degrees='true'"); EXPECT_PRED2(contains, elemStr, "rotation_format='euler_rpy'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -887,9 +887,11 @@ TEST(Pose1_9, ToStringWithQuatXYZ) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.7071068 0 0 0.7071068")); + // The string output has changed as it was parsed from the value, instead of + // the original string. std::string elemStr = poseElem->ToString(""); EXPECT_PRED2(contains, elemStr, "rotation_format='quat_xyzw'"); - EXPECT_PRED2(contains, elemStr, "0.7071068 0 0 0.7071068"); + EXPECT_PRED2(contains, elemStr, "0.707107 0 0 0.707107"); } ////////////////////////////////////////////////// @@ -915,10 +917,12 @@ TEST(Pose1_9, ToStringWithQuatXYZWDegreesFalse) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.7071068 0 0 0.7071068")); + // The string output has changed as it was parsed from the value, instead of + // the original string. std::string elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='0'"); + EXPECT_PRED2(contains, elemStr, "degrees='false'"); EXPECT_PRED2(contains, elemStr, "rotation_format='quat_xyzw'"); - EXPECT_PRED2(contains, elemStr, "0.7071068 0 0 0.7071068"); + EXPECT_PRED2(contains, elemStr, "0.707107 0 0 0.707107"); } ////////////////////////////////////////////////// @@ -941,18 +945,22 @@ TEST(Pose1_9, ToStringAfterChangingDegreeAttribute) std::string elemStr = poseElem->ToString(""); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); - // Changing to degrees + // Changing to attribute to degrees, however this does not modify the + // value of the underlying Param. Reparse needs to be called, which uses + // the input from SetFromString, to get a new value. sdf::ParamPtr degreesAttrib = poseElem->GetAttribute("degrees"); ASSERT_NE(nullptr, degreesAttrib); ASSERT_TRUE(degreesAttrib->Set(true)); + EXPECT_TRUE(valParam->Reparse()); + elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='1'"); + EXPECT_PRED2(contains, elemStr, "degrees='true'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); // Changing back to radians ASSERT_TRUE(degreesAttrib->Set(false)); elemStr = poseElem->ToString(""); - EXPECT_PRED2(contains, elemStr, "degrees='0'"); + EXPECT_PRED2(contains, elemStr, "degrees='false'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } diff --git a/test/integration/print_config.cc b/test/integration/print_config.cc index a3cc9aba1..ded642ab2 100644 --- a/test/integration/print_config.cc +++ b/test/integration/print_config.cc @@ -187,6 +187,9 @@ R"( auto *includeMergedModel = world->ModelByIndex(0); ASSERT_NE(includeMergedModel, nullptr); + // The expected output pose string here still contains a -0 on the pitch value + // as it was set using ignition::math::Pose3d::operator<<, this test will have + // to be modified when we start using ignitionrobotics/ign-math#206. const std::string expectedIncludeMerge = R"( diff --git a/test/sdf/includes_rotations_in_degrees.sdf b/test/sdf/includes_rotations_in_degrees.sdf new file mode 100644 index 000000000..ae04da1d6 --- /dev/null +++ b/test/sdf/includes_rotations_in_degrees.sdf @@ -0,0 +1,12 @@ + + + + + + test_model + parent_model + 1 2 3 30.009 44.991 -60.009 + + + + diff --git a/test/sdf/includes_rotations_in_quaternions.sdf b/test/sdf/includes_rotations_in_quaternions.sdf new file mode 100644 index 000000000..08795bd22 --- /dev/null +++ b/test/sdf/includes_rotations_in_quaternions.sdf @@ -0,0 +1,14 @@ + + + + + + test_model + parent_model + + 1 2 3 0.391948 0.200425 -0.532046 0.723279 + + + + + diff --git a/test/sdf/includes_rotations_in_radians.sdf b/test/sdf/includes_rotations_in_radians.sdf new file mode 100644 index 000000000..ef91f408a --- /dev/null +++ b/test/sdf/includes_rotations_in_radians.sdf @@ -0,0 +1,12 @@ + + + + + + test_model + parent_model + 1 2 3 0.523756 0.785241 -1.04735 + + + + diff --git a/test/sdf/rotations_in_degrees.sdf b/test/sdf/rotations_in_degrees.sdf new file mode 100644 index 000000000..9dbba3c44 --- /dev/null +++ b/test/sdf/rotations_in_degrees.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 30.009 44.991 -60.009 + + + diff --git a/test/sdf/rotations_in_degrees_high_snap_tolerance.sdf b/test/sdf/rotations_in_degrees_high_snap_tolerance.sdf new file mode 100644 index 000000000..273564354 --- /dev/null +++ b/test/sdf/rotations_in_degrees_high_snap_tolerance.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 30 48.5 60 + + + diff --git a/test/sdf/rotations_in_quaternions.sdf b/test/sdf/rotations_in_quaternions.sdf new file mode 100644 index 000000000..2a8447f38 --- /dev/null +++ b/test/sdf/rotations_in_quaternions.sdf @@ -0,0 +1,10 @@ + + + + + + 1 2 3 0.391948 0.200425 -0.532046 0.723279 + + + + diff --git a/test/sdf/rotations_in_radians.sdf b/test/sdf/rotations_in_radians.sdf new file mode 100644 index 000000000..36a218bd8 --- /dev/null +++ b/test/sdf/rotations_in_radians.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 0.523756 0.785241 -1.04735 + + + diff --git a/test/sdf/rotations_in_unnormalized_degrees.sdf b/test/sdf/rotations_in_unnormalized_degrees.sdf new file mode 100644 index 000000000..ba688c750 --- /dev/null +++ b/test/sdf/rotations_in_unnormalized_degrees.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 390.009 764.991 -420.009 + + + diff --git a/test/sdf/rotations_in_unnormalized_radians.sdf b/test/sdf/rotations_in_unnormalized_radians.sdf new file mode 100644 index 000000000..4d9578932 --- /dev/null +++ b/test/sdf/rotations_in_unnormalized_radians.sdf @@ -0,0 +1,8 @@ + + + + + 1 2 3 -5.75943 -11.78112 5.23583 + + + From 5f02f475471b0c10e84e86f768e12751bfa7be13 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 27 Dec 2021 12:30:26 -0800 Subject: [PATCH 60/60] Fix codecov badge (#805) Signed-off-by: Louise Poubel --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a86931709..4555101f3 100644 --- a/README.md +++ b/README.md @@ -32,7 +32,7 @@ TODO(eric.cousineau): Move terminology section to sdf_tutorials? ## Test coverage -[![codecov](https://codecov.io/gh/ignitionrobotics/sdformat/branch/sdf12/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/sdformat/branch/sdf12) +[![codecov](https://codecov.io/gh/ignitionrobotics/sdformat/branch/main/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/sdformat/branch/main) # Installation