[sdformat] 01/01: Imported Upstream version 3.6.0
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Thu Nov 12 15:48:27 UTC 2015
This is an automated email from the git hooks/post-receive script.
jrivero-guest pushed a commit to annotated tag upstream/3.6.0
in repository sdformat.
commit 7d1381907f382553b573e6420aace325c98c1c98
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Thu Nov 12 15:47:41 2015 +0000
Imported Upstream version 3.6.0
---
.hg_archival.txt | 6 +-
.hgtags | 7 +
CMakeLists.txt | 4 +-
Changelog.md | 17 ++
cmake/DefaultCFlags.cmake | 4 +-
cmake/SDFUtils.cmake | 63 ++++--
cmake/SearchForStuff.cmake | 12 ++
configure.bat | 8 +-
doc/header.html | 2 +-
doc/mainpage.html | 8 +-
include/sdf/Console.hh | 7 +-
include/sdf/Element.hh | 21 +-
include/sdf/Param.hh | 2 +-
sdf/1.5/CMakeLists.txt | 6 +-
sdf/1.5/battery.sdf | 12 ++
sdf/1.5/camera.sdf | 40 +++-
sdf/1.5/light_state.sdf | 12 ++
sdf/1.5/link.sdf | 3 +-
sdf/1.5/link_state.sdf | 46 ++++
sdf/1.5/model.sdf | 9 +-
sdf/1.5/model_state.sdf | 38 ++++
sdf/1.5/state.sdf | 69 +-----
sdf/1.5/surface.sdf | 60 +++++-
src/Console.cc | 2 +-
src/Element.cc | 91 ++++----
src/Param.cc | 5 +-
src/SDF_TEST.cc | 22 --
src/parser.cc | 18 +-
test/integration/CMakeLists.txt | 6 +-
.../cfm_damping_implicit_spring_damper.cc | 27 ---
test/integration/disable_fixed_joint_reduction.cc | 15 --
test/integration/element_memory_leak.cc | 95 ++++++++
test/integration/fixed_joint_reduction.cc | 27 ---
test/integration/force_torque_sensor.cc | 14 --
test/integration/joint_axis_frame.cc | 27 ---
test/integration/locale_fix.cc | 16 --
test/integration/nested_model.cc | 238 +++++++++++++++++++++
test/integration/parser_error_detection.cc | 14 --
test/integration/plugin_attribute.cc | 14 --
test/integration/plugin_bool.cc | 14 --
test/integration/provide_feedback.cc | 14 --
test/integration/urdf_joint_parameters.cc | 14 --
tools/code_check.sh | 8 +-
tools/get_mem_info.py | 12 ++
44 files changed, 773 insertions(+), 376 deletions(-)
diff --git a/.hg_archival.txt b/.hg_archival.txt
index 6025a28..c7dd77e 100644
--- a/.hg_archival.txt
+++ b/.hg_archival.txt
@@ -1,5 +1,5 @@
repo: 17049bd77df5bd6cd56a76edba4a54afb5647740
-node: 3739cf5051a6ddf7e8e13696c1ba1aeed8e2b80b
+node: 0a8c9f3ab18c2aeda4c0a561ece7a9c3fbe61ded
branch: sdf3
-latesttag: sdformat3_3.1.0
-latesttagdistance: 7
+latesttag: sdformat3_3.5.0
+latesttagdistance: 8
diff --git a/.hgtags b/.hgtags
index ff6434b..fac3e8d 100644
--- a/.hgtags
+++ b/.hgtags
@@ -95,3 +95,10 @@ a86c5d1fd79620b9e5c6558cec394d656c361e48 sdformat3-prerelease_3.0.2
345d293c0dd73bd2c35a0f92083d28fc250db355 sdformat3-prerelease_3.0.7
0825b2d6a1ca4658b22213b9c79010bf043b3576 sdformat3_3.0.0
c59a46c0e8f0869d84a0ee1fd9807f1e3b70f2cc sdformat3_3.1.0
+3739cf5051a6ddf7e8e13696c1ba1aeed8e2b80b sdformat3_3.1.1
+cc9d2f84bc213a2cfd422d5fa613ed6590d9bfd0 sdformat3_3.2.0
+cc9d2f84bc213a2cfd422d5fa613ed6590d9bfd0 sdformat3_3.2.0
+b26bb75fa846b25bedd6726615fd7fb6877b65df sdformat3_3.2.0
+a2a653f9545b08cbf4da18bf6aff441daa95447f sdformat3_3.3.0
+54b4f790ab1feac11f100928159ddf1aa50200e0 sdformat3_3.4.0
+346771c498661f0e773a0a8a3ecd877d2b0a000c sdformat3_3.5.0
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5489e40..56caf8e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -28,8 +28,8 @@ string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
set (SDF_PROTOCOL_VERSION 1.5)
set (SDF_MAJOR_VERSION 3)
-set (SDF_MINOR_VERSION 1)
-set (SDF_PATCH_VERSION 1)
+set (SDF_MINOR_VERSION 6)
+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 0b2beb9..a329838 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -2,6 +2,21 @@
### SDFormat 3.x.x (xxxx-xx-xx)
+1. Added lens element to camera SDF
+ * [Pull request 215](https://bitbucket.org/osrf/sdformat/pull-request/215)
+1. Added torsional friction to SDF
+ * [Pull request 211](https://bitbucket.org/osrf/sdformat/pull-request/211)
+ * [Pull request 217](https://bitbucket.org/osrf/sdformat/pull-request/217)
+
+1. Added support for nested models
+ * [Pull request 221](https://bitbucket.org/osrf/sdformat/pull-request/221)
+ * [Pull request 223](https://bitbucket.org/osrf/sdformat/pull-request/223)
+
+### SDFormat 3.1.1 (2015-08-03)
+
+1. Fix tinyxml linking error
+ * [Pull request #209](https://bitbucket.org/osrf/sdformat/pull-request/209)
+
### SDFormat 3.1.0 (2015-08-02)
1. Added logical camera sensor to SDF
@@ -9,6 +24,8 @@
### SDFormat 3.0.0 (2015-07-24)
+1. Added battery to SDF
+ * [Pull request 204](https://bitbucket.org/osrf/sdformat/pull-request/204)
1. Added altimeter sensor to SDF
* [Pull request 197](https://bitbucket.org/osrf/sdformat/pull-request/197)
1. Added magnetometer sensor to SDF
diff --git a/cmake/DefaultCFlags.cmake b/cmake/DefaultCFlags.cmake
index 0903872..0eea595 100644
--- a/cmake/DefaultCFlags.cmake
+++ b/cmake/DefaultCFlags.cmake
@@ -63,8 +63,8 @@ if ("${CMAKE_CXX_COMPILER_ID}" MATCHES "GNU")
elseif ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++")
elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
- if (NOT MSVC12)
- message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 os greater.")
+ if (MSVC_VERSION LESS 1800)
+ message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 or greater.")
endif()
else ()
message(FATAL_ERROR "Your C++ compiler does not support C++11.")
diff --git a/cmake/SDFUtils.cmake b/cmake/SDFUtils.cmake
index 5a83283..a3aa467 100644
--- a/cmake/SDFUtils.cmake
+++ b/cmake/SDFUtils.cmake
@@ -7,7 +7,7 @@ MACRO (APPEND_TO_CACHED_STRING _string _cacheDesc)
ENDFOREACH (newItem ${ARGN})
#STRING(STRIP ${${_string}} ${_string})
ENDMACRO (APPEND_TO_CACHED_STRING)
-
+
################################################################################
# APPEND_TO_CACHED_LIST (_list _cacheDesc [items...]
# Appends items to a cached list.
@@ -94,7 +94,7 @@ endmacro()
#################################################
macro (sdf_setup_windows)
# Need for M_PI constant
- add_definitions(-D_USE_MATH_DEFINES -DWINDOWS_LEAN_AND_MEAN)
+ add_definitions(-D_USE_MATH_DEFINES -DWINDOWS_LEAN_AND_MEAN)
# Suppress warnings caused by boost
add_definitions(/wd4512 /wd4996)
# Use dynamic linking for boost
@@ -105,7 +105,7 @@ macro (sdf_setup_windows)
if (MSVC AND CMAKE_SIZEOF_VOID_P EQUAL 8)
# Not need if proper cmake gnerator (-G "...Win64") is passed to cmake
# Enable as a second measeure to workaround over bug
- # http://www.cmake.org/Bug/print_bug_page.php?bug_id=11240
+ # http://www.cmake.org/Bug/print_bug_page.php?bug_id=11240
set(CMAKE_SHARED_LINKER_FLAGS "/machine:x64")
endif()
endmacro()
@@ -126,9 +126,9 @@ macro (sdf_build_tests)
if (UNIX)
add_executable(${BINARY_NAME} ${GTEST_SOURCE_file})
elseif(WIN32)
- add_executable(${BINARY_NAME}
+ add_executable(${BINARY_NAME}
${GTEST_SOURCE_file}
- ${PROJECT_SOURCE_DIR}/src/win/tinyxml/tinystr.cpp
+ ${PROJECT_SOURCE_DIR}/src/win/tinyxml/tinystr.cpp
${PROJECT_SOURCE_DIR}/src/win/tinyxml/tinyxmlerror.cpp
${PROJECT_SOURCE_DIR}/src/win/tinyxml/tinyxml.cpp
${PROJECT_SOURCE_DIR}/src/win/tinyxml/tinyxmlparser.cpp
@@ -140,7 +140,7 @@ macro (sdf_build_tests)
add_dependencies(${BINARY_NAME}
gtest gtest_main sdformat
)
-
+
link_directories(${IGNITION-MATH_LIBRARY_DIRS})
if (UNIX)
@@ -158,27 +158,34 @@ macro (sdf_build_tests)
gtest_main.lib
sdformat.dll
${IGNITION-MATH_LIBRARIES}
+ ${Boost_LIBRARIES}
)
endif()
-
+
add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME}
--gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
-
- set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240)
-
- # Check that the test produced a result and create a failure if it didn't.
- # Guards against crashed and timed out tests.
- add_test(check_${BINARY_NAME} python ${PROJECT_SOURCE_DIR}/tools/check_test_ran.py
- ${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
+
+ set(_env_vars)
+ list(APPEND _env_vars "SDF_PATH=${PROJECT_SOURCE_DIR}/sdf/${SDF_PROTOCOL_VERSION}")
+ set_tests_properties(${BINARY_NAME} PROPERTIES
+ TIMEOUT 240
+ ENVIRONMENT "${_env_vars}")
+
+ if(PYTHONINTERP_FOUND)
+ # Check that the test produced a result and create a failure if it didn't.
+ # Guards against crashed and timed out tests.
+ add_test(check_${BINARY_NAME} ${PYTHON_EXECUTABLE} ${PROJECT_SOURCE_DIR}/tools/check_test_ran.py
+ ${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
+ endif()
endforeach()
endmacro()
#################################################
# Macro to setup supported compiler warnings
-# Based on work of Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST.
+# Based on work of Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST.
include(CheckCXXCompilerFlag)
-macro(filter_valid_compiler_warnings)
+macro(filter_valid_compiler_warnings)
foreach(flag ${ARGN})
CHECK_CXX_COMPILER_FLAG(${flag} R${flag})
if(${R${flag}})
@@ -186,3 +193,27 @@ macro(filter_valid_compiler_warnings)
endif()
endforeach()
endmacro()
+
+#################################################
+# Copied from catkin/cmake/empy.cmake
+function(find_python_module module)
+ # cribbed from http://www.cmake.org/pipermail/cmake/2011-January/041666.html
+ string(TOUPPER ${module} module_upper)
+ if(NOT PY_${module_upper})
+ if(ARGC GREATER 1 AND ARGV1 STREQUAL "REQUIRED")
+ set(${module}_FIND_REQUIRED TRUE)
+ endif()
+ # A module's location is usually a directory, but for
+ # binary modules
+ # it's a .so file.
+ execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" "import re, ${module}; print(re.compile('/__init__.py.*').sub('',${module}.__file__))"
+ RESULT_VARIABLE _${module}_status
+ OUTPUT_VARIABLE _${module}_location
+ ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
+ if(NOT _${module}_status)
+ set(PY_${module_upper} ${_${module}_location} CACHE STRING "Location of Python module ${module}")
+ endif(NOT _${module}_status)
+ endif(NOT PY_${module_upper})
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(PY_${module} DEFAULT_MSG PY_${module_upper})
+endfunction(find_python_module)
diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake
index 73942a9..73bd031 100644
--- a/cmake/SearchForStuff.cmake
+++ b/cmake/SearchForStuff.cmake
@@ -90,6 +90,18 @@ else()
endif()
################################################
+# Find the Python interpreter for running the
+# check_test_ran.py script
+find_package(PythonInterp QUIET)
+
+################################################
+# Find psutil python package for memory tests
+find_python_module(psutil)
+if(NOT PY_PSUTIL)
+ BUILD_WARNING("Python psutil package not found. Memory leak tests will be skipped")
+endif()
+
+################################################
# Find ruby executable to produce xml schemas
find_program(RUBY ruby)
if (NOT RUBY)
diff --git a/configure.bat b/configure.bat
index 677344e..35ee96c 100644
--- a/configure.bat
+++ b/configure.bat
@@ -3,10 +3,10 @@
@echo Configuring for build type %build_type%
cmake -G "NMake Makefiles"^
- -DBOOST_ROOT:STRING="..\boost_1_56_0"^
- -DBOOST_LIBRARYDIR:STRING="..\boost_1_56_0\lib64-msvc-12.0"^
+ -DBOOST_ROOT:STRING="%cd%\..\..\boost_1_56_0"^
+ -DBOOST_LIBRARYDIR:STRING="%cd%\..\..\boost_1_56_0\lib64-msvc-12.0"^
-DCMAKE_INSTALL_PREFIX="install/%build_type%"^
- -DIGNITION-MATH_INCLUDE_DIRS:STRING="..\..\ign-math\build\install\%build_type%\include\ignition\math2"^
- -DIGNITION-MATH_LIBRARY_DIRS:STRING="..\..\ign-math\build\install\%build_type%\lib"^
+ -DIGNITION-MATH_INCLUDE_DIRS:STRING="%cd%\..\..\ign-math\build\install\%build_type%\include\ignition\math2"^
+ -DIGNITION-MATH_LIBRARY_DIRS:STRING="%cd%\..\..\ign-math\build\install\%build_type%\lib"^
-DIGNITION-MATH_LIBRARIES="ignition-math2"^
-DCMAKE_BUILD_TYPE="%build_type%" ..
diff --git a/doc/header.html b/doc/header.html
index f5aefc3..e0cfd8c 100644
--- a/doc/header.html
+++ b/doc/header.html
@@ -34,7 +34,7 @@
<div class="menu">
<dl>
<dt>Links</dt>
- <dd><a href="http://sdf.com">SDF Website</a></dd>
+ <dd><a href="http://sdformat.org">SDF Website</a></dd>
<!--
<dd><a href="http://sdf.com/wiki">Wiki</a></dd>
<dd><a href="http://sdf.com/wiki/Tutorials">Tutorials</a></dd>
diff --git a/doc/mainpage.html b/doc/mainpage.html
index c8ebda3..11ed373 100644
--- a/doc/mainpage.html
+++ b/doc/mainpage.html
@@ -20,13 +20,13 @@ Priority - minor; Version - blank.
<dl>
<dt>Links</dt>
- <dd><a href="http://sdf.com/">Website</a>: The main SDF website.</dd>
+ <dd><a href="http://sdformat.org">Website</a>: The main SDF website.</dd>
- <dd><a href="http://sdf.com/wiki">Wiki</a>: A collection of user supported documentation.</dd>
+ <dd><a href="http://sdformat.org/spec">Specification</a>: Documentation of sdformat elements.</dd>
- <dd><a href="http://sdf.com/wiki/tutorials">Tutorials</a>: Tutorials that describe how to use SDF.</dd>
+ <dd><a href="http://sdformat.org/tutorials">Tutorials</a>: Tutorials that describe how to use SDF.</dd>
- <dd><a href="http://sdf.com/downloads.html">Download</a>: How to download and install SDF</dd>
+ <dd><a href="http://sdformat.org/download">Download</a>: How to download and install SDF</dd>
</dl>
diff --git a/include/sdf/Console.hh b/include/sdf/Console.hh
index cbc85a5..c87329b 100644
--- a/include/sdf/Console.hh
+++ b/include/sdf/Console.hh
@@ -48,6 +48,11 @@ namespace sdf
__FILE__, __LINE__, 31))
class ConsolePrivate;
+ class Console;
+
+ /// \def ConsolePtr
+ /// \brief Shared pointer to a Console Element
+ typedef boost::shared_ptr<Console> ConsolePtr;
/// \brief Message, error, warning, and logging functionality
class SDFORMAT_VISIBLE Console
@@ -88,7 +93,7 @@ namespace sdf
public: virtual ~Console();
/// \brief Return an instance to this class.
- public: static boost::shared_ptr<Console> Instance();
+ public: static ConsolePtr Instance();
/// \brief Set quiet output
/// \param[in] q True to prevent warning
diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh
index 51cc52e..f1f6bac 100644
--- a/include/sdf/Element.hh
+++ b/include/sdf/Element.hh
@@ -53,6 +53,10 @@ namespace sdf
/// \brief boost shared pointer to an SDF Element
typedef boost::shared_ptr<Element> ElementPtr;
+ /// \def ElementWeakPtr
+ /// \brief Weak pointer to an SDF Element
+ typedef boost::weak_ptr<Element> ElementWeakPtr;
+
/// \def ElementPtr_V
/// \brief Vector of ElementPtr
typedef std::vector< ElementPtr > ElementPtr_V;
@@ -73,7 +77,7 @@ namespace sdf
/// \brief Create a copy of this Element.
/// \return A copy of this Element.
- public: boost::shared_ptr<Element> Clone() const;
+ public: ElementPtr Clone() const;
/// \brief Copy values from an Element.
/// \param[in] _elem Element to copy value from.
@@ -119,6 +123,14 @@ namespace sdf
/// \return True to copy child elements during parsing.
public: bool GetCopyChildren() const;
+ /// \brief Set reference SDF element.
+ /// \param[in] _value Name of the reference sdf element.
+ public: void SetReferenceSDF(const std::string &_value);
+
+ /// \brief Get the name of the reference SDF element.
+ /// \return Name of the reference SDF element.
+ public: std::string ReferenceSDF() const;
+
/// \brief Output Element's description to stdout.
/// \param[in] _prefix String value to prefix to the output.
public: void PrintDescription(const std::string &_prefix);
@@ -257,7 +269,7 @@ namespace sdf
std::ostringstream &_out) const;
- private: boost::shared_ptr<Param> CreateParam(const std::string &_key,
+ private: ParamPtr CreateParam(const std::string &_key,
const std::string &_type, const std::string &_defaultValue,
bool _required, const std::string &_description="");
@@ -283,7 +295,7 @@ namespace sdf
public: bool copyChildren;
/// \brief Element's parent
- public: ElementPtr parent;
+ public: ElementWeakPtr parent;
// Attributes of this element
public: Param_V attributes;
@@ -299,6 +311,9 @@ namespace sdf
/// name of the include file that was used to create this element
public: std::string includeFilename;
+
+ /// \brief Name of reference sdf.
+ public: std::string referenceSDF;
};
///////////////////////////////////////////////
diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh
index e8ae059..51ed27e 100644
--- a/include/sdf/Param.hh
+++ b/include/sdf/Param.hh
@@ -125,7 +125,7 @@ namespace sdf
/// \brief Clone the parameter.
/// \return A new parameter that is the clone of this.
- public: boost::shared_ptr<Param> Clone() const;
+ public: ParamPtr Clone() const;
/// \brief Set the update function. The updateFunc will be used to
/// set the parameter's value when Param::Update is called.
diff --git a/sdf/1.5/CMakeLists.txt b/sdf/1.5/CMakeLists.txt
index 751a378..38bfc52 100644
--- a/sdf/1.5/CMakeLists.txt
+++ b/sdf/1.5/CMakeLists.txt
@@ -3,6 +3,7 @@ set (sdfs
altimeter.sdf
audio_source.sdf
audio_sink.sdf
+ battery.sdf
box_shape.sdf
camera.sdf
collision.sdf
@@ -19,12 +20,15 @@ set (sdfs
inertial.sdf
joint.sdf
light.sdf
+ light_state.sdf
link.sdf
+ link_state.sdf
logical_camera.sdf
magnetometer.sdf
material.sdf
mesh_shape.sdf
model.sdf
+ model_state.sdf
noise.sdf
physics.sdf
plane_shape.sdf
@@ -69,6 +73,6 @@ endforeach()
add_custom_target(schema ALL DEPENDS ${SDF_SCHEMA})
set_source_files_properties(${SDF_SCHEMA} PROPERTIES GENERATED TRUE)
-
+
install(FILES 1_4.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.5)
install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.5)
diff --git a/sdf/1.5/battery.sdf b/sdf/1.5/battery.sdf
new file mode 100644
index 0000000..dc6f8de
--- /dev/null
+++ b/sdf/1.5/battery.sdf
@@ -0,0 +1,12 @@
+<!-- Battery -->
+<element name="battery" required="*">
+ <description>Description of a battery.</description>
+
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>Unique name for the battery.</description>
+ </attribute>
+
+ <element name="voltage" type="double" default="0.0" required="1">
+ <description>Initial voltage in volts.</description>
+ </element>
+</element>
diff --git a/sdf/1.5/camera.sdf b/sdf/1.5/camera.sdf
index d3b87c1..9192e13 100644
--- a/sdf/1.5/camera.sdf
+++ b/sdf/1.5/camera.sdf
@@ -9,7 +9,7 @@
<description>A position and orientation in the parent coordinate frame for the camera.</description>
</element>
- <element name="horizontal_fov" type="double" default="1.047" min="0.1" max="1.5708" required="1">
+ <element name="horizontal_fov" type="double" default="1.047" min="0.1" max="6.283186" required="1">
<description>Horizontal field of view</description>
</element> <!-- End Horizontal_FOV -->
@@ -90,4 +90,42 @@
</element>
</element> <!-- End Distortion -->
+ <element name="lens" required="0">
+ <description>Lens projection description</description>
+
+ <element name="type" type="string" default="stereographic" required="1">
+ <description>Type of the lens mapping. Supported values are gnomonical, stereographic, equidistant, equisolid_angle, orthographic, custom. For gnomonical (perspective) projection, it is recommended to specify a horizontal_fov of less than or equal to 90°</description>
+ </element>
+ <element name="scale_to_hfov" type="bool" default="true" required="1">
+ <description>If true the image will be scaled to fit horizontal FOV, otherwise it will be shown according to projection type parameters</description>
+ </element>
+
+ <element name="custom_function" required="0">
+ <description>Definition of custom mapping function in a form of r=c1*f*fun(theta/c2 + c3). See https://en.wikipedia.org/wiki/Fisheye_lens#Mapping_function</description>
+ <element name="c1" type="double" default="1" required="0">
+ <description>Linear scaling constant</description>
+ </element>
+ <element name="c2" type="double" default="1" required="0">
+ <description>Angle scaling constant</description>
+ </element>
+ <element name="c3" type="double" default="0" required="0">
+ <description>Angle offset constant</description>
+ </element>
+ <element name="f" type="double" default="1" required="0">
+ <description>Focal length of the optical system. Note: It's not a focal length of the lens in a common sense! This value is ignored if 'scale_to_fov' is set to true</description>
+ </element>
+ <element name="fun" type="string" default="tan" required="1">
+ <description>Possible values are 'sin', 'tan' and 'id'</description>
+ </element>
+ </element> <!-- End Custom Function -->
+
+ <element name="cutoff_angle" type="double" default="1.5707" min="0.0" max="3.141592653" required="0">
+ <description>Everything outside of the specified angle will be hidden, 90° by default</description>
+ </element>
+
+ <element name="env_texture_size" type="int" default="256" min="4" max="2048" required="0">
+ <description>Resolution of the environment cube map used to draw the world</description>
+ </element>
+
+ </element> <!-- End Lens -->
</element> <!-- End Camera -->
diff --git a/sdf/1.5/light_state.sdf b/sdf/1.5/light_state.sdf
new file mode 100644
index 0000000..9614cfc
--- /dev/null
+++ b/sdf/1.5/light_state.sdf
@@ -0,0 +1,12 @@
+<!-- State information for a light -->
+<element name="light" required="*">
+ <description>Light state</description>
+
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>Name of the light</description>
+ </attribute>
+
+ <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
+ <description>Pose of the light</description>
+ </element>
+</element> <!-- End Light -->
diff --git a/sdf/1.5/link.sdf b/sdf/1.5/link.sdf
index e00d8ee..e2c6ca9 100644
--- a/sdf/1.5/link.sdf
+++ b/sdf/1.5/link.sdf
@@ -1,5 +1,5 @@
<!-- Link -->
-<element name="link" required="+">
+<element name="link" required="*">
<description>A physical link with inertia, collision, and visual properties. A link must be a child of a model, and any number of links may exist in a model.</description>
<attribute name="name" type="string" default="__default__" required="1">
@@ -43,5 +43,6 @@
<include filename="projector.sdf" required="*"/>
<include filename="audio_sink.sdf" required="*"/>
<include filename="audio_source.sdf" required="*"/>
+ <include filename="battery.sdf" required="*"/>
</element> <!-- End Link -->
diff --git a/sdf/1.5/link_state.sdf b/sdf/1.5/link_state.sdf
new file mode 100644
index 0000000..c324fb9
--- /dev/null
+++ b/sdf/1.5/link_state.sdf
@@ -0,0 +1,46 @@
+<!-- State information for a link -->
+<element name="link" required="*">
+ <description>Link state</description>
+
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>Name of the link</description>
+ </attribute>
+
+ <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
+ <description>Pose of the link relative to the model</description>
+ </element>
+
+ <element name="velocity" type="pose" default="0 0 0 0 0 0" required="0">
+ <description>Velocity of the link. The x, y, z components of the pose
+ correspond to the linear velocity of the link, and the roll, pitch, yaw
+ components correspond to the angular velocity of the link
+ </description>
+ </element>
+
+ <element name="acceleration" type="pose" default="0 0 0 0 0 0" required="0">
+ <description>Acceleration of the link. The x, y, z components of the pose
+ correspond to the linear acceleration of the link, and the roll,
+ pitch, yaw components correspond to the angular acceleration of the link
+ </description>
+ </element>
+
+ <element name="wrench" type="pose" default="0 0 0 0 0 0" required="0">
+ <description>Force and torque applied to the link. The x, y, z components
+ of the pose correspond to the force applied to the link, and the roll,
+ pitch, yaw components correspond to the torque applied to the link
+ </description>
+ </element>
+
+ <element name="collision" required="*">
+ <description>Collision state</description>
+
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>Name of the collision</description>
+ </attribute>
+
+ <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
+ <description>Pose of the link relative to the model</description>
+ </element>
+ </element>
+
+</element> <!-- End Link -->
diff --git a/sdf/1.5/model.sdf b/sdf/1.5/model.sdf
index 3c07330..8c3765c 100644
--- a/sdf/1.5/model.sdf
+++ b/sdf/1.5/model.sdf
@@ -22,7 +22,7 @@
<description>A position and orientation in the global coordinate frame for the model. Position(x,y,z) and rotation (roll, pitch yaw) in the global coordinate frame.</description>
</element>
- <include filename="link.sdf" required="+"/>
+ <include filename="link.sdf" required="*"/>
<include filename="joint.sdf" required="*"/>
<include filename="plugin.sdf" required="*"/>
<include filename="gripper.sdf" required="*"/>
@@ -46,4 +46,11 @@
</element>
</element>
+ <element name="model" ref="model" required="*">
+ <description>A nested model element</description>
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>A unique name for the model. This name must not match another nested model in the same level as this model.</description>
+ </attribute>
+ </element>
+
</element> <!-- End Model -->
diff --git a/sdf/1.5/model_state.sdf b/sdf/1.5/model_state.sdf
new file mode 100644
index 0000000..cb3b6db
--- /dev/null
+++ b/sdf/1.5/model_state.sdf
@@ -0,0 +1,38 @@
+<!-- State information for a model -->
+<element name="model" required="*">
+ <description>Model state</description>
+
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>Name of the model</description>
+ </attribute>
+
+ <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
+ <description>Pose of the model</description>
+ </element>
+
+ <element name="joint" required="*">
+ <description>Joint angle</description>
+
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>Name of the joint</description>
+ </attribute>
+
+ <element name="angle" type="double" default="0" required="+">
+ <attribute name="axis" type="unsigned int" default="0" required="1">
+ <description>Index of the axis.</description>
+ </attribute>
+
+ <description>Angle of an axis</description>
+ </element>
+ </element>
+
+ <element name="model" ref="model_state" required="*">
+ <description>A nested model state element</description>
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>Name of the model. </description>
+ </attribute>
+ </element>
+
+ <include filename="link_state.sdf" required="*"/>
+
+</element> <!-- End Model -->
diff --git a/sdf/1.5/state.sdf b/sdf/1.5/state.sdf
index fff058b..9e23d81 100644
--- a/sdf/1.5/state.sdf
+++ b/sdf/1.5/state.sdf
@@ -33,73 +33,8 @@
</element>
</element>
+ <include filename="model_state.sdf" required="*"/>
- <!-- State information for a model -->
- <element name="model" required="*">
- <description>Model state</description>
+ <include filename="light_state.sdf" required="*"/>
- <attribute name="name" type="string" default="__default__" required="1">
- <description>Name of the model</description>
- </attribute>
-
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the model</description>
- </element>
-
- <element name="joint" required="*">
- <description>Joint angle</description>
-
- <attribute name="name" type="string" default="__default__" required="1">
- <description>Name of the joint</description>
- </attribute>
-
- <element name="angle" type="double" default="0" required="+">
- <attribute name="axis" type="unsigned int" default="0" required="1">
- <description>Index of the axis.</description>
- </attribute>
-
- <description>Angle of an axis</description>
- </element>
- </element>
-
- <!-- State information for a link -->
- <element name="link" required="*">
- <description>Link state</description>
-
- <attribute name="name" type="string" default="__default__" required="1">
- <description>Name of the link</description>
- </attribute>
-
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the link relative to the model</description>
- </element>
-
- <element name="velocity" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Velocity of the link</description>
- </element>
-
- <element name="acceleration" type="pose"
- default="0 0 0 0 0 0" required="0">
- <description>Acceleration of the link</description>
- </element>
-
- <element name="wrench" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Force applied to the link</description>
- </element>
-
- <element name="collision" required="*">
- <description>Collision state</description>
-
- <attribute name="name" type="string" default="__default__" required="1">
- <description>Name of the collision</description>
- </attribute>
-
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the link relative to the model</description>
- </element>
- </element>
-
-
- </element> <!-- End Link -->
- </element> <!-- End Model -->
</element> <!-- End State -->
diff --git a/sdf/1.5/surface.sdf b/sdf/1.5/surface.sdf
index 5d28752..7545103 100644
--- a/sdf/1.5/surface.sdf
+++ b/sdf/1.5/surface.sdf
@@ -12,6 +12,35 @@
<element name="friction" required="0">
<description></description>
+
+ <element name="torsional" required="0">
+ <description>Parameters for torsional friction</description>
+ <element name="coefficient" type="double" default="1.0" min="0.0" required="0">
+ <description>Torsional friction coefficient in the range of [0..1].</description>
+ </element>
+ <element name="use_patch_radius" type="bool" default="1" required="0">
+ <description>
+ If this flag is true,
+ torsional friction is calculated using the "patch_radius" parameter.
+ If this flag is set to false,
+ "surface_radius" (R) and contact depth (d)
+ are used to compute the patch radius as sqrt(R*d).
+ </description>
+ </element>
+ <element name="patch_radius" type="double" default="0" min="0.0" required="0">
+ <description>Radius of contact patch surface.</description>
+ </element>
+ <element name="surface_radius" type="double" default="0.0" min="0.0" required="0">
+ <description>Surface radius on the point of contact.</description>
+ </element>
+ <element name="ode" required="0">
+ <description>Torsional friction parameters for ODE</description>
+ <element name="slip" type="double" default="0.0" required="0">
+ <description>Force dependent slip for torsional friction, between the range of [0..1].</description>
+ </element>
+ </element> <!-- End ODE -->
+ </element> <!-- End torsional -->
+
<element name="ode" required="0">
<description>ODE friction parameters</description>
<element name="mu" type="double" default="1" min="0.0" required="0">
@@ -53,12 +82,41 @@
</element>
<element name="collide_without_contact_bitmask" type="unsigned int" default="1" required="0">
<description>Bitmask for collision filtering when collide_without_contact is on </description>
- </element>
+ </element>
<element name="collide_bitmask" type="unsigned int" default="65535" required="0">
<description>Bitmask for collision filtering. This will override collide_without_contact</description>
</element>
+ <element name="poissons_ratio" type="double" default="0.3" required="0">
+ <description>
+ Poisson's ratio is the ratio between transverse and axial strain.
+ This value must lie between (-1, 0.5). Defaults to 0.3 for typical steel.
+ Note typical silicone elastomers have Poisson's ratio near 0.49 ~ 0.50.
+
+ For reference, approximate values for Material:(Young's Modulus, Poisson's Ratio)
+ for some of the typical materials are:
+ Plastic: (1e8 ~ 3e9 Pa, 0.35 ~ 0.41),
+ Wood: (4e9 ~ 1e10 Pa, 0.22 ~ 0.50),
+ Aluminum: (7e10 Pa, 0.32 ~ 0.35),
+ Steel: (2e11 Pa, 0.26 ~ 0.31).
+ </description>
+ </element>
+ <element name="elastic_modulus" type="double" default="-1" required="0">
+ <description>
+ Young's Modulus in SI derived unit Pascal.
+ Defaults to -1. If value is less or equal to zero,
+ contact using elastic modulus (with Poisson's Ratio) is disabled.
+
+ For reference, approximate values for Material:(Young's Modulus, Poisson's Ratio)
+ for some of the typical materials are:
+ Plastic: (1e8 ~ 3e9 Pa, 0.35 ~ 0.41),
+ Wood: (4e9 ~ 1e10 Pa, 0.22 ~ 0.50),
+ Aluminum: (7e10 Pa, 0.32 ~ 0.35),
+ Steel: (2e11 Pa, 0.26 ~ 0.31).
+ </description>
+ </element>
+
<element name="ode" required="0">
<description>ODE contact parameters</description>
<element name="soft_cfm" type="double" default="0" required="0">
diff --git a/src/Console.cc b/src/Console.cc
index cf4b14e..dd2e21f 100644
--- a/src/Console.cc
+++ b/src/Console.cc
@@ -84,7 +84,7 @@ Console::~Console()
}
//////////////////////////////////////////////////
-boost::shared_ptr<Console> Console::Instance()
+ConsolePtr Console::Instance()
{
std::lock_guard<std::mutex> lock(g_instance_mutex);
if (!myself)
diff --git a/src/Element.cc b/src/Element.cc
index f6e4e9f..0c78994 100644
--- a/src/Element.cc
+++ b/src/Element.cc
@@ -25,45 +25,20 @@ Element::Element()
: dataPtr(new ElementPrivate)
{
this->dataPtr->copyChildren = false;
+ this->dataPtr->referenceSDF = "";
}
/////////////////////////////////////////////////
Element::~Element()
{
- this->dataPtr->parent.reset();
- for (Param_V::iterator iter = this->dataPtr->attributes.begin();
- iter != this->dataPtr->attributes.end(); ++iter)
- {
- (*iter).reset();
- }
- this->dataPtr->attributes.clear();
-
- for (ElementPtr_V::iterator iter = this->dataPtr->elements.begin();
- iter != this->dataPtr->elements.end(); ++iter)
- {
- (*iter).reset();
- }
-
- for (ElementPtr_V::iterator iter = this->dataPtr->elementDescriptions.begin();
- iter != this->dataPtr->elementDescriptions.end(); ++iter)
- {
- (*iter).reset();
- }
- this->dataPtr->elements.clear();
- this->dataPtr->elementDescriptions.clear();
-
- this->dataPtr->value.reset();
-
delete this->dataPtr;
this->dataPtr = NULL;
-
- // this->Reset();
}
/////////////////////////////////////////////////
ElementPtr Element::GetParent() const
{
- return this->dataPtr->parent;
+ return this->dataPtr->parent.lock();
}
/////////////////////////////////////////////////
@@ -109,6 +84,18 @@ bool Element::GetCopyChildren() const
}
/////////////////////////////////////////////////
+void Element::SetReferenceSDF(const std::string &_value)
+{
+ this->dataPtr->referenceSDF = _value;
+}
+
+/////////////////////////////////////////////////
+std::string Element::ReferenceSDF() const
+{
+ return this->dataPtr->referenceSDF;
+}
+
+/////////////////////////////////////////////////
void Element::AddValue(const std::string &_type,
const std::string &_defaultValue, bool _required,
const std::string &_description)
@@ -145,6 +132,7 @@ ElementPtr Element::Clone() const
// clone->parent = this->dataPtr->parent;
clone->dataPtr->copyChildren = this->dataPtr->copyChildren;
clone->dataPtr->includeFilename = this->dataPtr->includeFilename;
+ clone->dataPtr->referenceSDF = this->dataPtr->referenceSDF;
Param_V::const_iterator aiter;
for (aiter = this->dataPtr->attributes.begin();
@@ -181,6 +169,7 @@ void Element::Copy(const ElementPtr _elem)
this->dataPtr->required = _elem->GetRequired();
this->dataPtr->copyChildren = _elem->GetCopyChildren();
this->dataPtr->includeFilename = _elem->dataPtr->includeFilename;
+ this->dataPtr->referenceSDF = _elem->ReferenceSDF();
for (Param_V::iterator iter = _elem->dataPtr->attributes.begin();
iter != _elem->dataPtr->attributes.end(); ++iter)
@@ -243,6 +232,13 @@ void Element::PrintDescription(const std::string &_prefix)
if (this->GetCopyChildren())
std::cout << _prefix << " <element copy_data ='true' required ='*'/>\n";
+ std::string refSDF = this->ReferenceSDF();
+ if (!refSDF.empty())
+ {
+ std::cout << _prefix << " <element ref ='" << refSDF <<
+ "' required ='*'/>\n";
+ }
+
ElementPtr_V::iterator eiter;
for (eiter = this->dataPtr->elementDescriptions.begin();
eiter != this->dataPtr->elementDescriptions.end(); ++eiter)
@@ -579,25 +575,26 @@ ElementPtr Element::GetFirstElement() const
/////////////////////////////////////////////////
ElementPtr Element::GetNextElement(const std::string &_name) const
{
- if (this->dataPtr->parent)
+ auto parent = this->dataPtr->parent.lock();
+ if (parent)
{
ElementPtr_V::const_iterator iter;
- iter = std::find(this->dataPtr->parent->dataPtr->elements.begin(),
- this->dataPtr->parent->dataPtr->elements.end(), shared_from_this());
+ iter = std::find(parent->dataPtr->elements.begin(),
+ parent->dataPtr->elements.end(), shared_from_this());
- if (iter == this->dataPtr->parent->dataPtr->elements.end())
+ if (iter == parent->dataPtr->elements.end())
{
return ElementPtr();
}
++iter;
- if (iter == this->dataPtr->parent->dataPtr->elements.end())
+ if (iter == parent->dataPtr->elements.end())
return ElementPtr();
else if (_name.empty())
return *(iter);
else
{
- for (; iter != this->dataPtr->parent->dataPtr->elements.end(); ++iter)
+ for (; iter != parent->dataPtr->elements.end(); ++iter)
{
if ((*iter)->GetName() == _name)
return (*iter);
@@ -644,6 +641,20 @@ bool Element::HasElementDescription(const std::string &_name)
/////////////////////////////////////////////////
ElementPtr Element::AddElement(const std::string &_name)
{
+ // if this element is a reference sdf and does not have any element
+ // descriptions then get them from its parent
+ auto parent = this->dataPtr->parent.lock();
+ if (!this->dataPtr->referenceSDF.empty() &&
+ this->dataPtr->elementDescriptions.empty() && parent &&
+ parent->GetName() == this->dataPtr->name)
+ {
+ for (unsigned int i = 0; i < parent->GetElementDescriptionCount(); ++i)
+ {
+ this->dataPtr->elementDescriptions.push_back(
+ parent->GetElementDescription(i)->Clone());
+ }
+ }
+
ElementPtr_V::const_iterator iter, iter2;
for (iter = this->dataPtr->elementDescriptions.begin();
iter != this->dataPtr->elementDescriptions.end(); ++iter)
@@ -668,6 +679,7 @@ ElementPtr Element::AddElement(const std::string &_name)
return this->dataPtr->elements.back();
}
}
+
sdferr << "Missing element description for [" << _name << "]\n";
return ElementPtr();
}
@@ -762,16 +774,17 @@ void Element::SetDescription(const std::string &_desc)
/////////////////////////////////////////////////
void Element::RemoveFromParent()
{
- if (this->dataPtr->parent)
+ auto parent = this->dataPtr->parent.lock();
+ if (parent)
{
ElementPtr_V::iterator iter;
- iter = std::find(this->dataPtr->parent->dataPtr->elements.begin(),
- this->dataPtr->parent->dataPtr->elements.end(), shared_from_this());
+ iter = std::find(parent->dataPtr->elements.begin(),
+ parent->dataPtr->elements.end(), shared_from_this());
- if (iter != this->dataPtr->parent->dataPtr->elements.end())
+ if (iter != parent->dataPtr->elements.end())
{
- this->dataPtr->parent->dataPtr->elements.erase(iter);
- this->dataPtr->parent.reset();
+ parent->dataPtr->elements.erase(iter);
+ parent.reset();
}
}
}
diff --git a/src/Param.cc b/src/Param.cc
index 3ced5eb..cd7cf67 100644
--- a/src/Param.cc
+++ b/src/Param.cc
@@ -477,10 +477,9 @@ void Param::Reset()
}
//////////////////////////////////////////////////
-boost::shared_ptr<Param> Param::Clone() const
+ParamPtr Param::Clone() const
{
- return boost::shared_ptr<Param>(
- new Param(this->dataPtr->key, this->dataPtr->typeName,
+ return ParamPtr(new Param(this->dataPtr->key, this->dataPtr->typeName,
this->GetAsString(), this->dataPtr->required,
this->dataPtr->description));
}
diff --git a/src/SDF_TEST.cc b/src/SDF_TEST.cc
index 734e735..8245ec7 100644
--- a/src/SDF_TEST.cc
+++ b/src/SDF_TEST.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include <boost/any.hpp>
#include <ignition/math.hh>
#include "test_config.h"
@@ -28,32 +27,11 @@ class SDFUpdate : public testing::Test
{
protected: SDFUpdate()
{
- boost::filesystem::path path =
- boost::filesystem::path(PROJECT_SOURCE_PATH)
- / "sdf" / SDF_VERSION;
-
- // Store original env var.
-#ifndef _WIN32
- this->origSDFPath = getenv("SDF_PATH");
-#else
- this->origSDFPath = const_cast<char*>(
- sdf::winGetEnv("SDF_PATH"));
-#endif
-
- setenv("SDF_PATH", path.string().c_str(), 1);
}
protected: virtual ~SDFUpdate()
{
- // Restore original env var.
- // osx segfaults unless this check is in place
- // some discussion of portability of setenv at:
- // http://www.greenend.org.uk/rjk/tech/putenv.html
- if (this->origSDFPath)
- setenv("SDF_PATH", this->origSDFPath, 1);
}
-
- private: char *origSDFPath;
};
class SDFUpdateFixture
diff --git a/src/parser.cc b/src/parser.cc
index 222d86b..ef11208 100644
--- a/src/parser.cc
+++ b/src/parser.cc
@@ -152,6 +152,10 @@ bool initDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf)
//////////////////////////////////////////////////
bool initXml(TiXmlElement *_xml, ElementPtr _sdf)
{
+ const char *refString = _xml->Attribute("ref");
+ if (refString)
+ _sdf->SetReferenceSDF(std::string(refString));
+
const char *nameString = _xml->Attribute("name");
if (!nameString)
{
@@ -486,6 +490,18 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf)
_sdf->GetValue()->SetFromString(_xml->GetText());
}
+ // check for nested sdf
+ std::string refSDFStr = _sdf->ReferenceSDF();
+ if (!refSDFStr.empty())
+ {
+ ElementPtr refSDF;
+ refSDF.reset(new Element);
+ std::string refFilename = refSDFStr + ".sdf";
+ initFile(refFilename, refSDF);
+ _sdf->RemoveFromParent();
+ _sdf->Copy(refSDF);
+ }
+
TiXmlAttribute *attribute = _xml->FirstAttribute();
unsigned int i = 0;
@@ -756,7 +772,7 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf)
}
}
- // Chek that all required elements have been set
+ // Check that all required elements have been set
unsigned int descCounter = 0;
for (descCounter = 0;
descCounter != _sdf->GetElementDescriptionCount(); ++descCounter)
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 77d3eee..85fdcaf 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -8,6 +8,7 @@ set(tests
force_torque_sensor.cc
joint_axis_frame.cc
locale_fix.cc
+ nested_model.cc
parser_error_detection.cc
plugin_attribute.cc
plugin_bool.cc
@@ -15,6 +16,10 @@ set(tests
urdf_joint_parameters.cc
)
+if (PYTHONINTERP_FOUND AND PY_PSUTIL)
+ set(tests ${tests} element_memory_leak.cc)
+endif()
+
find_program(XMLLINT_EXE xmllint)
if (EXISTS ${XMLLINT_EXE})
set (tests ${tests} schema_test.cc)
@@ -30,4 +35,3 @@ if (EXISTS ${XMLLINT_EXE})
# Need to run schema target (build .xsd files) before running schema_test
add_dependencies(${TEST_TYPE}_schema_test schema)
endif()
-
diff --git a/test/integration/cfm_damping_implicit_spring_damper.cc b/test/integration/cfm_damping_implicit_spring_damper.cc
index 97a72fe..1c56b38 100644
--- a/test/integration/cfm_damping_implicit_spring_damper.cc
+++ b/test/integration/cfm_damping_implicit_spring_damper.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include <map>
#include "sdf/sdf.hh"
@@ -31,22 +30,9 @@ const std::string SDF_TEST_FILE = std::string(PROJECT_SOURCE_PATH)
/////////////////////////////////////////////////
TEST(SDFParser, CFMDampingSDFTest)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE, robot));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
sdf::ElementPtr model = robot->Root()->GetElement("model");
for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
@@ -104,22 +90,9 @@ TEST(SDFParser, CFMDampingSDFTest)
/////////////////////////////////////////////////
TEST(SDFParser, CFMDampingURDFTest)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(URDF_TEST_FILE, robot));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
sdf::ElementPtr model = robot->Root()->GetElement("model");
for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
diff --git a/test/integration/disable_fixed_joint_reduction.cc b/test/integration/disable_fixed_joint_reduction.cc
index 1425d3c..98e2f82 100644
--- a/test/integration/disable_fixed_joint_reduction.cc
+++ b/test/integration/disable_fixed_joint_reduction.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include <map>
#include "sdf/sdf.hh"
@@ -50,11 +49,6 @@ bool findJointInModel(std::string desired_joint_name, sdf::SDFPtr robot)
/////////////////////////////////////////////////
TEST(SDFParser, DisableFixedJointReductionTest)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(SDF_FIXED_JNT, robot));
@@ -63,15 +57,6 @@ TEST(SDFParser, DisableFixedJointReductionTest)
sdf::init(robot_disable_lumping);
ASSERT_TRUE(sdf::readFile(SDF_FIXED_JNT_NO_LUMPING, robot_disable_lumping));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
-
ASSERT_FALSE(findJointInModel("joint12a", robot));
ASSERT_FALSE(findJointInModel("joint23a", robot));
diff --git a/test/integration/element_memory_leak.cc b/test/integration/element_memory_leak.cc
new file mode 100644
index 0000000..c127108
--- /dev/null
+++ b/test/integration/element_memory_leak.cc
@@ -0,0 +1,95 @@
+/*
+ * Copyright 2015 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 <iostream>
+#include <gtest/gtest.h>
+#include "sdf/sdf.hh"
+#include "sdf/parser_urdf.hh"
+
+#include "test_config.h"
+
+/////////////////////////////////////////////////
+std::string custom_exec(std::string _cmd)
+{
+#ifdef _WIN32
+ FILE *pipe = _popen(_cmd.c_str(), "r");
+#else
+ FILE *pipe = popen(_cmd.c_str(), "r");
+#endif
+
+ if (!pipe)
+ return "ERROR";
+
+ char buffer[128];
+ std::string result = "";
+
+ while (!feof(pipe))
+ {
+ if (fgets(buffer, 128, pipe) != NULL)
+ result += buffer;
+ }
+
+#ifdef _WIN32
+ _pclose(pipe);
+#else
+ pclose(pipe);
+#endif
+
+ return result;
+}
+
+const std::string sdfString(
+ "<?xml version='1.0'?>\n"
+ "<sdf version='1.5'>\n"
+ " <model name='break_bot'>\n"
+ " <link name='root-link'>\n"
+ " <sensor type='gps' name='mysensor' />"
+ " </link>\n"
+ " </model>\n"
+ "</sdf>");
+
+const std::string pythonMeminfo("python "
+ PROJECT_SOURCE_PATH "/tools/get_mem_info.py");
+
+int getMemoryUsage()
+{
+ return std::stoi(custom_exec(pythonMeminfo));
+}
+
+//////////////////////////////////////////////////
+TEST(ElementMemoryLeak, SDFCreateDestroy)
+{
+ // Initial memory usage
+ int memoryLimit = getMemoryUsage();
+ std::cout << "initial memory: " << memoryLimit << std::endl;
+
+ // Allow 15x increase (based on testing with Ubuntu and OSX)
+ memoryLimit *= 15;
+
+ for (unsigned int i = 0; i < 50; ++i)
+ {
+ sdf::SDF modelSDF;
+ modelSDF.SetFromString(sdfString);
+
+ int memoryUsage = getMemoryUsage();
+ EXPECT_LT(memoryUsage, memoryLimit);
+ }
+
+ std::cout << " final memory: "
+ << getMemoryUsage()
+ << std::endl;
+}
diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc
index 9a19b7a..9d5ee77 100644
--- a/test/integration/fixed_joint_reduction.cc
+++ b/test/integration/fixed_joint_reduction.cc
@@ -17,7 +17,6 @@
#include <gtest/gtest.h>
#include <map>
-#include <boost/filesystem.hpp>
#include "sdf/sdf.hh"
#include "test_config.h"
@@ -65,22 +64,9 @@ TEST(SDFParser, FixedJointReductionCollisionTest)
/////////////////////////////////////////////////
void FixedJointReductionEquivalence(const std::string &_file)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(_file, robot));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
std::map<std::string, double> linkMasses;
std::map<std::string, ignition::math::Pose3d> linkPoses;
@@ -212,22 +198,9 @@ void FixedJointReductionEquivalence(const std::string &_file)
/////////////////////////////////////////////////
TEST(SDFParser, FixedJointReductionSimple)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE_SIMPLE, robot));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
std::map<std::string, double> linkMasses;
std::map<std::string, ignition::math::Pose3d> linkPoses;
diff --git a/test/integration/force_torque_sensor.cc b/test/integration/force_torque_sensor.cc
index d74a281..59ab717 100644
--- a/test/integration/force_torque_sensor.cc
+++ b/test/integration/force_torque_sensor.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include <map>
#include "sdf/sdf.hh"
@@ -28,22 +27,9 @@ const std::string SDF_TEST_FILE = std::string(PROJECT_SOURCE_PATH)
/////////////////////////////////////////////////
TEST(SDFParser, ForceTorqueSensorTest)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE, robot));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
sdf::ElementPtr model = robot->Root()->GetElement("model");
for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
diff --git a/test/integration/joint_axis_frame.cc b/test/integration/joint_axis_frame.cc
index 741dae2..27a4dbc 100644
--- a/test/integration/joint_axis_frame.cc
+++ b/test/integration/joint_axis_frame.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include <string>
#include "sdf/sdf.hh"
@@ -47,22 +46,9 @@ std::string get_sdf_string(std::string _version)
// expect tag to be inserted with value true
TEST(JointAxisFrame, Version_1_4_missing)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr model(new sdf::SDF());
sdf::init(model);
ASSERT_TRUE(sdf::readString(get_sdf_string("1.4"), model));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
sdf::ElementPtr joint = model->Root()->GetElement(
"model")->GetElement("joint");
@@ -78,22 +64,9 @@ TEST(JointAxisFrame, Version_1_4_missing)
// expect tag to be inserted with value false
TEST(JointAxisFrame, Version_1_5_missing)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr model(new sdf::SDF());
sdf::init(model);
ASSERT_TRUE(sdf::readString(get_sdf_string("1.5"), model));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
sdf::ElementPtr joint = model->Root()->GetElement(
"model")->GetElement("joint");
diff --git a/test/integration/locale_fix.cc b/test/integration/locale_fix.cc
index 3d688d7..4ff411b 100644
--- a/test/integration/locale_fix.cc
+++ b/test/integration/locale_fix.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include "sdf/sdf.hh"
#include "sdf/parser_urdf.hh"
@@ -51,25 +50,10 @@ TEST(CheckFixForLocal, MakeTestToFail)
setlocale(LC_NUMERIC, line);
// fix to allow make test without make install
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr p(new sdf::SDF());
sdf::init(p);
ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE, p));
- // clean up environment variables that we modified
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
-
sdf::ElementPtr elem = p->Root()->GetElement("world")
->GetElement("physics")->GetElement("ode")->GetElement("solver")
->GetElement("sor");
diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc
new file mode 100644
index 0000000..c4f05db
--- /dev/null
+++ b/test/integration/nested_model.cc
@@ -0,0 +1,238 @@
+/*
+ * Copyright 2015 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 <gtest/gtest.h>
+#include <string>
+#include "sdf/sdf.hh"
+
+#include "test_config.h"
+
+////////////////////////////////////////
+// Test parsing nested model with joint
+TEST(NestedModel, NestedModel)
+{
+ std::ostringstream stream;
+ std::string version = "1.5";
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='top_level_model'>"
+ << " <link name='parent'/>"
+ << " <link name='child'/>"
+ << " <model name='nested_model'>"
+ << " <link name='nested_link01'/>"
+ << " </model>"
+ << " <joint name='top_level_joint' type='revolute'>"
+ << " <parent>parent</parent>"
+ << " <child>child</child>"
+ << " <axis>"
+ << " <xyz>1 0 0</xyz>"
+ << " </axis>"
+ << " </joint>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // top level model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "top_level_model");
+
+ // top level links
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linklElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linklElem->HasAttribute("name"));
+ EXPECT_EQ(linklElem->Get<std::string>("name"), "parent");
+ linklElem = linklElem->GetNextElement("link");
+ EXPECT_TRUE(linklElem != NULL);
+ EXPECT_TRUE(linklElem->HasAttribute("name"));
+ EXPECT_EQ(linklElem->Get<std::string>("name"), "child");
+
+ // nested model
+ EXPECT_TRUE(modelElem->HasElement("model"));
+ sdf::ElementPtr nestedModelElem = modelElem->GetElement("model");
+
+ // nested model link
+ EXPECT_TRUE(nestedModelElem->HasElement("link"));
+ sdf::ElementPtr nestedLinkElem = nestedModelElem->GetElement("link");
+ EXPECT_TRUE(nestedLinkElem->HasAttribute("name"));
+ EXPECT_EQ(nestedLinkElem->Get<std::string>("name"), "nested_link01");
+
+ // top level model joint
+ EXPECT_TRUE(modelElem->HasElement("joint"));
+ sdf::ElementPtr jointElem = modelElem->GetElement("joint");
+ EXPECT_TRUE(jointElem->HasAttribute("name"));
+ EXPECT_EQ(jointElem->Get<std::string>("name"), "top_level_joint");
+ EXPECT_TRUE(jointElem->HasAttribute("type"));
+ EXPECT_EQ(jointElem->Get<std::string>("type"), "revolute");
+
+ // joint links
+ EXPECT_TRUE(jointElem->HasElement("parent"));
+ EXPECT_EQ(jointElem->Get<std::string>("parent"), "parent");
+ EXPECT_TRUE(jointElem->HasElement("child"));
+ EXPECT_EQ(jointElem->Get<std::string>("child"), "child");
+
+ // joint axis
+ EXPECT_TRUE(jointElem->HasElement("axis"));
+ sdf::ElementPtr axisElem = jointElem->GetElement("axis");
+
+ EXPECT_TRUE(axisElem->HasElement("xyz"));
+ EXPECT_EQ(axisElem->Get<ignition::math::Vector3d>("xyz"),
+ ignition::math::Vector3d(1, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing nested model states
+TEST(NestedModel, State)
+{
+ std::ostringstream sdfStr;
+ sdfStr << "<sdf version ='" << SDF_VERSION << "'>"
+ << "<world name='default'>"
+ << "<state world_name='default'>"
+ << "<model name='model_00'>"
+ << " <pose>0 0 0.5 0 0 0</pose>"
+ << " <link name='link_00'>"
+ << " <pose>0 0 0.5 0 0 0</pose>"
+ << " <velocity>0.001 0 0 0 0 0</velocity>"
+ << " <acceleration>0 0.006121 0 0.012288 0 0.001751</acceleration>"
+ << " <wrench>0 0.006121 0 0 0 0</wrench>"
+ << " </link>"
+ << " <model name='model_01'>"
+ << " <pose>1 0 0.5 0 0 0</pose>"
+ << " <link name='link_01'>"
+ << " <pose>1.25 0 0.5 0 0 0</pose>"
+ << " <velocity>0 -0.001 0 0 0 0</velocity>"
+ << " <acceleration>0 0.000674 0 -0.001268 0 0</acceleration>"
+ << " <wrench>0 0.000674 0 0 0 0</wrench>"
+ << " </link>"
+ << " <model name='model_02'>"
+ << " <pose>1 1 0.5 0 0 0</pose>"
+ << " <link name='link_02'>"
+ << " <pose>1.25 1 0.5 0 0 0</pose>"
+ << " <velocity>0 0 0.001 0 0 0</velocity>"
+ << " <acceleration>0 0 0 0 0 0</acceleration>"
+ << " <wrench>0 0 0 0 0 0</wrench>"
+ << " </link>"
+ << " </model>"
+ << " </model>"
+ << "</model>"
+ << "</state>"
+ << "</world>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(sdfStr.str(), sdfParsed));
+
+ // load the state sdf
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("world"));
+ sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world");
+ EXPECT_TRUE(worldElem->HasElement("state"));
+ sdf::ElementPtr stateElem = worldElem->GetElement("state");
+ EXPECT_TRUE(stateElem->HasElement("model"));
+
+ sdf::ElementPtr modelStateElem = stateElem->GetElement("model");
+
+ // model sdf
+ EXPECT_TRUE(modelStateElem->HasAttribute("name"));
+ EXPECT_EQ(modelStateElem->Get<std::string>("name"), "model_00");
+ EXPECT_TRUE(modelStateElem->HasElement("pose"));
+ EXPECT_EQ(modelStateElem->Get<ignition::math::Pose3d>("pose"),
+ ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0));
+ EXPECT_TRUE(!modelStateElem->HasElement("joint"));
+
+ // link sdf
+ EXPECT_TRUE(modelStateElem->HasElement("link"));
+ sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link");
+ EXPECT_TRUE(linkStateElem->HasAttribute("name"));
+ EXPECT_EQ(linkStateElem->Get<std::string>("name"), "link_00");
+ EXPECT_TRUE(linkStateElem->HasElement("pose"));
+ EXPECT_EQ(linkStateElem->Get<ignition::math::Pose3d>("pose"),
+ ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0));
+ EXPECT_TRUE(linkStateElem->HasElement("velocity"));
+ EXPECT_EQ(linkStateElem->Get<ignition::math::Pose3d>("velocity"),
+ ignition::math::Pose3d(0.001, 0, 0, 0, 0, 0));
+ EXPECT_TRUE(linkStateElem->HasElement("acceleration"));
+ EXPECT_EQ(linkStateElem->Get<ignition::math::Pose3d>("acceleration"),
+ ignition::math::Pose3d(0, 0.006121, 0, 0.012288, 0, 0.001751));
+ EXPECT_TRUE(linkStateElem->HasElement("wrench"));
+ EXPECT_EQ(linkStateElem->Get<ignition::math::Pose3d>("wrench"),
+ ignition::math::Pose3d(0, 0.006121, 0, 0, 0, 0));
+
+ // nested model sdf
+ EXPECT_TRUE(modelStateElem->HasElement("model"));
+ sdf::ElementPtr nestedModelStateElem =
+ modelStateElem->GetElement("model");
+ EXPECT_TRUE(nestedModelStateElem->HasAttribute("name"));
+ EXPECT_EQ(nestedModelStateElem->Get<std::string>("name"), "model_01");
+ EXPECT_TRUE(nestedModelStateElem->HasElement("pose"));
+ EXPECT_EQ(nestedModelStateElem->Get<ignition::math::Pose3d>("pose"),
+ ignition::math::Pose3d(1, 0, 0.5, 0, 0, 0));
+ EXPECT_TRUE(!nestedModelStateElem->HasElement("joint"));
+
+ // nested model's link sdf
+ EXPECT_TRUE(nestedModelStateElem->HasElement("link"));
+ sdf::ElementPtr nestedLinkStateElem =
+ nestedModelStateElem->GetElement("link");
+ EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name"));
+ EXPECT_EQ(nestedLinkStateElem->Get<std::string>("name"), "link_01");
+ EXPECT_TRUE(nestedLinkStateElem->HasElement("pose"));
+ EXPECT_EQ(nestedLinkStateElem->Get<ignition::math::Pose3d>("pose"),
+ ignition::math::Pose3d(1.25, 0, 0.5, 0, 0, 0));
+ EXPECT_TRUE(nestedLinkStateElem->HasElement("velocity"));
+ EXPECT_EQ(nestedLinkStateElem->Get<ignition::math::Pose3d>("velocity"),
+ ignition::math::Pose3d(0, -0.001, 0, 0, 0, 0));
+ EXPECT_TRUE(nestedLinkStateElem->HasElement("acceleration"));
+ EXPECT_EQ(nestedLinkStateElem->Get<ignition::math::Pose3d>("acceleration"),
+ ignition::math::Pose3d(0, 0.000674, 0, -0.001268, 0, 0));
+ EXPECT_TRUE(nestedLinkStateElem->HasElement("wrench"));
+ EXPECT_EQ(nestedLinkStateElem->Get<ignition::math::Pose3d>("wrench"),
+ ignition::math::Pose3d(0, 0.000674, 0, 0, 0, 0));
+
+ // double nested model sdf
+ EXPECT_TRUE(nestedModelStateElem->HasElement("model"));
+ nestedModelStateElem = nestedModelStateElem->GetElement("model");
+ EXPECT_TRUE(nestedModelStateElem->HasAttribute("name"));
+ EXPECT_EQ(nestedModelStateElem->Get<std::string>("name"), "model_02");
+ EXPECT_TRUE(nestedModelStateElem->HasElement("pose"));
+ EXPECT_EQ(nestedModelStateElem->Get<ignition::math::Pose3d>("pose"),
+ ignition::math::Pose3d(1, 1, 0.5, 0, 0, 0));
+ EXPECT_TRUE(!nestedModelStateElem->HasElement("joint"));
+
+ // double nested model's link sdf
+ EXPECT_TRUE(nestedModelStateElem->HasElement("link"));
+ nestedLinkStateElem = nestedModelStateElem->GetElement("link");
+ EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name"));
+ EXPECT_EQ(nestedLinkStateElem->Get<std::string>("name"), "link_02");
+ EXPECT_TRUE(nestedLinkStateElem->HasElement("pose"));
+ EXPECT_EQ(nestedLinkStateElem->Get<ignition::math::Pose3d>("pose"),
+ ignition::math::Pose3d(1.25, 1, 0.5, 0, 0, 0));
+ EXPECT_TRUE(nestedLinkStateElem->HasElement("velocity"));
+ EXPECT_EQ(nestedLinkStateElem->Get<ignition::math::Pose3d>("velocity"),
+ ignition::math::Pose3d(0, 0, 0.001, 0, 0, 0));
+ EXPECT_TRUE(nestedLinkStateElem->HasElement("acceleration"));
+ EXPECT_EQ(nestedLinkStateElem->Get<ignition::math::Pose3d>("acceleration"),
+ ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
+ EXPECT_TRUE(nestedLinkStateElem->HasElement("wrench"));
+ EXPECT_EQ(nestedLinkStateElem->Get<ignition::math::Pose3d>("wrench"),
+ ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
+}
diff --git a/test/integration/parser_error_detection.cc b/test/integration/parser_error_detection.cc
index d1a2995..2c78795 100644
--- a/test/integration/parser_error_detection.cc
+++ b/test/integration/parser_error_detection.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include <string>
#include "sdf/sdf.hh"
@@ -49,20 +48,7 @@ std::string get_sdf_string()
// make sure that XML errors get caught
TEST(ParserErrorDetection, BadXML)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr model(new sdf::SDF());
sdf::init(model);
ASSERT_FALSE(sdf::readString(get_sdf_string(), model));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
}
diff --git a/test/integration/plugin_attribute.cc b/test/integration/plugin_attribute.cc
index 9aae26f..97fd06c 100644
--- a/test/integration/plugin_attribute.cc
+++ b/test/integration/plugin_attribute.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include <string>
#include "sdf/sdf.hh"
@@ -42,22 +41,9 @@ std::string get_sdf_string()
// make sure that plugin attributes get parsed
TEST(PluginAttribute, ParseAttributes)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr model(new sdf::SDF());
sdf::init(model);
ASSERT_TRUE(sdf::readString(get_sdf_string(), model));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
sdf::ElementPtr plugin =
model->Root()->GetElement("model")->GetElement("plugin");
diff --git a/test/integration/plugin_bool.cc b/test/integration/plugin_bool.cc
index 2dfe792..8b98933 100644
--- a/test/integration/plugin_bool.cc
+++ b/test/integration/plugin_bool.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include <string>
#include "sdf/sdf.hh"
@@ -43,22 +42,9 @@ std::string get_sdf_string()
// make sure that we can read boolean values from inside a plugin
TEST(PluginBool, ParseBoolValue)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr model(new sdf::SDF());
sdf::init(model);
ASSERT_TRUE(sdf::readString(get_sdf_string(), model));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
sdf::ElementPtr plugin =
model->Root()->GetElement("model")->GetElement("plugin");
diff --git a/test/integration/provide_feedback.cc b/test/integration/provide_feedback.cc
index 99ce60a..536abb3 100644
--- a/test/integration/provide_feedback.cc
+++ b/test/integration/provide_feedback.cc
@@ -16,7 +16,6 @@
*/
#include <gtest/gtest.h>
-#include <boost/filesystem.hpp>
#include <map>
#include "sdf/sdf.hh"
@@ -28,22 +27,9 @@ const std::string SDF_TEST_FILE = std::string(PROJECT_SOURCE_PATH)
/////////////////////////////////////////////////
TEST(SDFParser, ProvideFeedbackTest)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE, robot));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
sdf::ElementPtr model = robot->Root()->GetElement("model");
for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
diff --git a/test/integration/urdf_joint_parameters.cc b/test/integration/urdf_joint_parameters.cc
index eccc71f..0e18fee 100644
--- a/test/integration/urdf_joint_parameters.cc
+++ b/test/integration/urdf_joint_parameters.cc
@@ -17,7 +17,6 @@
#include <gtest/gtest.h>
#include <map>
-#include <boost/filesystem.hpp>
#include "sdf/sdf.hh"
#include "test_config.h"
@@ -28,22 +27,9 @@ const std::string SDF_TEST_FILE = std::string(PROJECT_SOURCE_PATH)
/////////////////////////////////////////////////
TEST(SDFParser, JointAxisParameters)
{
- char *pathCStr = getenv("SDF_PATH");
- boost::filesystem::path path = PROJECT_SOURCE_PATH;
- path = path / "sdf" / SDF_VERSION;
- setenv("SDF_PATH", path.string().c_str(), 1);
-
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE, robot));
- if (pathCStr)
- {
- setenv("SDF_PATH", pathCStr, 1);
- }
- else
- {
- unsetenv("SDF_PATH");
- }
sdf::ElementPtr model = robot->Root()->GetElement("model");
diff --git a/tools/code_check.sh b/tools/code_check.sh
index 3f82336..5c9b1be 100755
--- a/tools/code_check.sh
+++ b/tools/code_check.sh
@@ -21,9 +21,9 @@ echo "*:src/Assert.cc" >> $SUPPRESS
echo "*:src/Console.cc" >> $SUPPRESS
echo "*:src/parser.cc" >> $SUPPRESS
echo "*:src/parser_urdf.cc" >> $SUPPRESS
-echo "*:src/Element.cc:467" >> $SUPPRESS
-echo "*:src/Element.cc:64" >> $SUPPRESS
-echo "*:src/Element.cc:739" >> $SUPPRESS
+echo "*:src/Element.cc:463" >> $SUPPRESS
+echo "*:src/Element.cc:39" >> $SUPPRESS
+echo "*:src/Element.cc:751" >> $SUPPRESS
CHECK_FILE_DIRS="./src ./include ./test/performance ./test/integration"
@@ -36,7 +36,7 @@ CPPCHECK_COMMAND1="-j 4 --enable=style,performance,portability,information $CPPC
# Unused function checking must happen in one job
CPPCHECK_COMMAND2="--enable=unusedFunction $CPPCHECK_FILES"
# -j 4 was used previously in CPPCHECK_COMMAND3 but it will generate a false
-# warning as described in bug:
+# warning as described in bug:
# http://sourceforge.net/apps/trac/cppcheck/ticket/4946
CPPCHECK_COMMAND3="-j 1 --enable=missingInclude --suppress=missingIncludeSystem $CPPCHECK_FILES $CPPCHECK_INCLUDES --check-config"
if [ $xmlout -eq 1 ]; then
diff --git a/tools/get_mem_info.py b/tools/get_mem_info.py
new file mode 100644
index 0000000..da9a34b
--- /dev/null
+++ b/tools/get_mem_info.py
@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+import os;
+import psutil;
+# get pid of parent process
+pid = os.getppid();
+# get and print memory usage of parent process
+p = psutil.Process(pid);
+# note that different versions of psutil have different API
+# some versions have memory_info(), others have get_memory_info()
+memory_info = (p.memory_info() if hasattr(p, 'memory_info') else p.get_memory_info());
+print memory_info[0]
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/sdformat.git
More information about the debian-science-commits
mailing list