[gazebo] 01/01: Imported Upstream version 5.1.0
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Tue Apr 14 22:19:12 UTC 2015
This is an automated email from the git hooks/post-receive script.
jrivero-guest pushed a commit to annotated tag upstream/5.1.0
in repository gazebo.
commit 95f16cfa34ed18613f714bb4cd00e6c4500c70ac
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Wed Mar 25 14:48:58 2015 +0100
Imported Upstream version 5.1.0
---
.hg_archival.txt | 4 +-
CMakeLists.txt | 16 +-
Changelog.md | 104 +-
cmake/SearchForStuff.cmake | 9 +-
cmake/gazebo-config.cmake.in | 2 +-
cmake/setup.sh.in | 6 +-
deps/opende/src/step.cpp | 8 +
doc/gazebo.in | 4 +-
doc/header.html | 3 +-
gazebo/common/AudioDecoder.cc | 4 +-
gazebo/common/CMakeLists.txt | 1 +
gazebo/common/Dem.cc | 5 +-
gazebo/common/DemPrivate.hh | 2 +-
gazebo/common/MeshManager.cc | 143 +--
gazebo/common/Video.cc | 2 +-
gazebo/common/{ffmpeg_inc.h => ffmpeg_inc.cc} | 37 +-
gazebo/common/ffmpeg_inc.h | 19 +-
gazebo/gui/BoxMaker.cc | 45 +-
gazebo/gui/CMakeLists.txt | 5 +-
gazebo/gui/CylinderMaker.cc | 48 +-
gazebo/gui/GLWidget.cc | 9 +-
gazebo/gui/MainWindow_TEST.cc | 124 +++
gazebo/gui/MainWindow_TEST.hh | 6 +
gazebo/gui/ModelManipulator.cc | 7 +-
gazebo/gui/RenderWidget.cc | 4 -
gazebo/gui/SphereMaker.cc | 40 +-
gazebo/math/Quaternion.hh | 73 +-
gazebo/math/Quaternion_TEST.cc | 87 ++
gazebo/math/SignalStats.cc | 20 +-
gazebo/msgs/msgs.cc | 558 ++++++++++-
gazebo/msgs/msgs.hh | 144 ++-
gazebo/msgs/msgs_TEST.cc | 749 +++++++++++++-
gazebo/physics/HeightmapShape.cc | 7 +-
gazebo/physics/Joint.cc | 15 +-
gazebo/physics/Link.cc | 6 +
gazebo/physics/Link.hh | 7 +
gazebo/physics/PhysicsIface.cc | 44 +-
gazebo/physics/World.cc | 185 ++--
gazebo/physics/bullet/BulletHingeJoint.cc | 6 +-
gazebo/physics/bullet/BulletJoint.cc | 14 +-
gazebo/physics/bullet/BulletLink.cc | 1 +
gazebo/physics/bullet/BulletSliderJoint.cc | 37 +-
gazebo/physics/dart/DARTJoint.cc | 98 +-
gazebo/physics/dart/DARTLink.cc | 23 +-
gazebo/physics/dart/dart_inc.h | 45 +-
gazebo/physics/simbody/SimbodyJoint.cc | 6 +-
gazebo/physics/simbody/SimbodyPhysics.cc | 8 +-
gazebo/physics/simbody/SimbodyScrewJoint.cc | 18 +-
gazebo/rendering/Camera.cc | 5 +-
gazebo/rendering/GpuLaser_TEST.cc | 1 +
gazebo/rendering/Heightmap.cc | 117 ++-
gazebo/rendering/JointVisual_TEST.cc | 1 +
gazebo/rendering/SonarVisual_TEST.cc | 1 +
gazebo/rendering/TransmitterVisual_TEST.cc | 1 +
gazebo/rendering/UserCamera.cc | 22 +-
.../rendering/selection_buffer/SelectionBuffer.cc | 2 +-
gazebo/sensors/ContactSensor.cc | 6 +-
gazebo/sensors/ImuSensor.cc | 4 +-
gazebo/sensors/Sensor_TEST.cc | 84 +-
gazebo/transport/Node.cc | 2 +-
test/ServerFixture.cc | 135 +--
test/ServerFixture.hh | 1059 ++++++++++----------
test/integration/CMakeLists.txt | 6 +-
test/integration/contact_sensor.cc | 79 +-
test/integration/gripper.cc | 1 +
test/integration/joint_spawn.cc | 88 +-
test/integration/joint_test.hh | 118 +--
test/integration/model.cc | 1 +
test/integration/ogre_log.cc | 1 +
test/integration/physics_collision.cc | 1 -
test/integration/physics_friction.cc | 160 ---
test/integration/physics_inertia_ratio.cc | 2 +-
test/integration/physics_link.cc | 264 +++++
test/integration/polyline.cc | 1 +
test/integration/pr2.cc | 6 +-
test/integration/sensor.cc | 1 +
test/integration/speed.cc | 46 +
test/integration/world.cc | 1 +
test/integration/world_clone.cc | 1 +
.../regression/351_world_step.cc | 33 +-
test/regression/602_unsubscribe_segfault.cc | 105 ++
test/regression/846_typo_in_camera.cc | 1 +
test/regression/CMakeLists.txt | 2 +
test/worlds/contact_sensors_multiple.world | 67 ++
test/worlds/ray_test.world | 3 +
test/worlds/revolute_joint_test.world | 1 -
test/worlds/world_step.world | 325 ++++++
tools/check_test_ran.py | 2 +-
tools/gz_TEST.cc | 12 +-
89 files changed, 3994 insertions(+), 1582 deletions(-)
diff --git a/.hg_archival.txt b/.hg_archival.txt
index d1ce13d..697e77c 100644
--- a/.hg_archival.txt
+++ b/.hg_archival.txt
@@ -1,5 +1,5 @@
repo: e803bb5fe03c1370cbb9f434912cd73fd1e7942d
-node: 430e66533f8810646b90d001fc9c83220ae503e9
+node: d115a95b5c947563ba641917895d0efbbbf82bc8
branch: gazebo5
-latesttag: gazebo5_5.0.0
+latesttag: gazebo4_4.1.2
latesttagdistance: 6
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 63ea6f7..9bdde1c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,11 +10,11 @@ string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
set (GAZEBO_MAJOR_VERSION 5)
-set (GAZEBO_MINOR_VERSION 0)
+set (GAZEBO_MINOR_VERSION 1)
# The patch version may have been bumped for prerelease purposes; be sure to
# check gazebo-release/ubuntu/debian/changelog at default to determine what the
# next patch version should be for a regular release.
-set (GAZEBO_PATCH_VERSION 1)
+set (GAZEBO_PATCH_VERSION 0)
set (GAZEBO_VERSION ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION})
set (GAZEBO_VERSION_FULL ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}.${GAZEBO_PATCH_VERSION})
@@ -181,8 +181,12 @@ endif()
#####################################
# Handle CFlags
+# USE_UPSTREAM_CFLAGS (default TRUE)
+if(NOT DEFINED USE_UPSTREAM_CFLAGS)
+ set (USE_UPSTREAM_CFLAGS True)
+endif()
+
unset (CMAKE_C_FLAGS_ALL CACHE)
-unset (CMAKE_CXX_FLAGS CACHE)
# USE_HOST_CFLAGS (default TRUE)
# Will check building host machine for proper cflags
@@ -191,9 +195,11 @@ if(NOT DEFINED USE_HOST_CFLAGS OR USE_HOST_CFLAGS)
include (${gazebo_cmake_dir}/HostCFlags.cmake)
endif()
-# USE_UPSTREAM_CFLAGS (default TRUE)
# Will use predefined gazebo developers cflags
-if(NOT DEFINED USE_UPSTREAM_CFLAGS OR USE_UPSTREAM_CFLAGS)
+# this needs to be called after HostCFlags
+if(USE_UPSTREAM_CFLAGS)
+ # use gazebo own set of flags
+ unset (CMAKE_CXX_FLAGS CACHE)
message(STATUS "Enable upstream CFlags")
include(${gazebo_cmake_dir}/DefaultCFlags.cmake)
endif()
diff --git a/Changelog.md b/Changelog.md
index 82e0657..2d86dc5 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,6 +1,47 @@
## Gazebo 5.0
-### Gazebo 5.0.0
+### Gazebo 5.1.0 (2015-03-20)
+1. Backport pull request #1527 (FindOGRE.cmake for non-Debian systems)
+ * [Pull request #1532](https://bitbucket.org/osrf/gazebo/pull-request/1532)
+
+1. Respect system cflags when not using USE_UPSTREAM_CFLAGS
+ * [Pull request #1531](https://bitbucket.org/osrf/gazebo/pull-request/1531)
+
+1. Allow light manipulation
+ * [Pull request #1529](https://bitbucket.org/osrf/gazebo/pull-request/1529)
+
+1. Allow sdformat 2.3.1+ or 3+ and fix tests
+ * [Pull request #1484](https://bitbucket.org/osrf/gazebo/pull-request/1484)
+
+1. Add Link::GetWorldAngularMomentum function and test.
+ * [Pull request #1482](https://bitbucket.org/osrf/gazebo/pull-request/1482)
+
+1. Preserve previous GAZEBO_MODEL_PATH values when sourcing setup.sh
+ * [Pull request #1430](https://bitbucket.org/osrf/gazebo/pull-request/1430)
+
+1. Implement Coulomb joint friction for DART
+ * [Pull request #1427](https://bitbucket.org/osrf/gazebo/pull-request/1427)
+ * [Issue #1281](https://bitbucket.org/osrf/gazebo/issue/1281)
+
+1. Fix simple shape normals.
+ * [Pull request #1477](https://bitbucket.org/osrf/gazebo/pull-request/1477)
+ * [Issue #1369](https://bitbucket.org/osrf/gazebo/issue/1369)
+
+1. Use Msg-to-SDF conversion functions in tests, add ServerFixture::SpawnModel(msgs::Model).
+ * [Pull request #1466](https://bitbucket.org/osrf/gazebo/pull-request/1466)
+
+1. Added Model Msg-to-SDF conversion functions and test.
+ * [Pull request #1429](https://bitbucket.org/osrf/gazebo/pull-request/1429)
+ * [Pull request #1419](https://bitbucket.org/osrf/gazebo/pull-request/1419)
+
+1. Added Visual, Material Msg-to-SDF conversion functions and ShaderType to string conversion functions.
+ * [Pull request #1415](https://bitbucket.org/osrf/gazebo/pull-request/1415)
+
+1. Implement Coulomb joint friction for BulletSliderJoint
+ * [Pull request #1452](https://bitbucket.org/osrf/gazebo/pull-request/1452)
+ * [Issue #1348](https://bitbucket.org/osrf/gazebo/issue/1348)
+
+### Gazebo 5.0.0 (2015-01-27)
1. Support for using [digital elevation maps](http://gazebosim.org/tutorials?tut=dem) has been added to debian packages.
1. C++11 support (C++11 compatible compiler is now required)
@@ -50,7 +91,7 @@
* [Issue #381](https://bitbucket.org/osrf/gazebo/issue/381)
1. Implement Coulomb joint friction for BulletHingeJoint
- * [Pull request #1221](https://bitbucket.org/osrf/gazebo/pull-request/1317)
+ * [Pull request #1317](https://bitbucket.org/osrf/gazebo/pull-request/1317)
* [Issue #1348](https://bitbucket.org/osrf/gazebo/issue/1348)
1. Implemented camera lens distortion.
@@ -96,6 +137,10 @@
1. Fixed crash on "permission denied" bug, added insert_model integration test.
* [Pull request #1329](https://bitbucket.org/osrf/gazebo/pull-request/1329/)
+1. Enable simbody joint tests, implement `SimbodyJoint::GetParam`, create
+ `Joint::GetParam`, fix bug in `BulletHingeJoint::SetParam`.
+ * [Pull request #1404](https://bitbucket.org/osrf/gazebo/pull-request/1404/)
+
1. Building editor updates
1. Fixed inspector resizing.
* [Pull request #1230](https://bitbucket.org/osrf/gazebo/pull-request/1230)
@@ -202,7 +247,60 @@
## Gazebo 4.0
-### Gazebo 4.x.x (yyyy-mm-dd)
+### Gazebo 4.1.2 (2015-03-20)
+
+1. Fix quaternion documentation: target Gazebo_4.1
+ * [Pull request #1525](https://bitbucket.org/osrf/gazebo/pull-request/1525)
+1. Speed up World::Step in loops
+ * [Pull request #1492](https://bitbucket.org/osrf/gazebo/pull-request/1492)
+1. Reduce selection buffer updates -> 4.1
+ * [Pull request #1494](https://bitbucket.org/osrf/gazebo/pull-request/1494)
+1. Fix QT rendering, and rendering update rate
+ * [Pull request #1487](https://bitbucket.org/osrf/gazebo/pull-request/1487)
+1. Fix loading of SimbodyPhysics parameters
+ * [Pull request #1474](https://bitbucket.org/osrf/gazebo/pull-request/1474)
+1. Fix heightmap on OSX -> 4.1
+ * [Pull request #1455](https://bitbucket.org/osrf/gazebo/pull-request/1455)
+1. Remove extra pose tag in a world file that should not be there
+ * [Pull request #1458](https://bitbucket.org/osrf/gazebo/pull-request/1458)
+1. Better fix for #236 for IMU that doesn't require ABI changes
+ * [Pull request #1448](https://bitbucket.org/osrf/gazebo/pull-request/1448)
+1. Fix regression of #236 for ImuSensor in 4.1
+ * [Pull request #1446](https://bitbucket.org/osrf/gazebo/pull-request/1446)
+1. Preserve previous GAZEBO_MODEL_PATH values when sourcing setup.sh
+ * [Pull request #1430](https://bitbucket.org/osrf/gazebo/pull-request/1430)
+1. issue #857: fix segfault for simbody screw joint when setting limits due to uninitialized limitForce.
+ * [Pull request #1423](https://bitbucket.org/osrf/gazebo/pull-request/1423)
+1. Allow multiple contact sensors per link (#960)
+ * [Pull request #1413](https://bitbucket.org/osrf/gazebo/pull-request/1413)
+1. Fix for issue #351, ODE World Step
+ * [Pull request #1406](https://bitbucket.org/osrf/gazebo/pull-request/1406)
+1. Disable failing InelasticCollision/0 test (#1394)
+ * [Pull request #1405](https://bitbucket.org/osrf/gazebo/pull-request/1405)
+1. Prevent out of bounds array access in SkidSteerDrivePlugin (found by cppcheck 1.68)
+ * [Pull request #1379](https://bitbucket.org/osrf/gazebo/pull-request/1379)
+
+### Gazebo 4.1.1 (2015-01-15)
+
+1. Fix BulletPlaneShape bounding box (#1265)
+ * [Pull request #1367](https://bitbucket.org/osrf/gazebo/pull-request/1367)
+1. Fix dart linking errors on osx
+ * [Pull request #1372](https://bitbucket.org/osrf/gazebo/pull-request/1372)
+1. Update to player interfaces
+ * [Pull request #1324](https://bitbucket.org/osrf/gazebo/pull-request/1324)
+1. Handle GpuLaser name collisions (#1403)
+ * [Pull request #1360](https://bitbucket.org/osrf/gazebo/pull-request/1360)
+1. Add checks for handling array's with counts of zero, and read specular values
+ * [Pull request #1339](https://bitbucket.org/osrf/gazebo/pull-request/1339)
+1. Fix model list widget test
+ * [Pull request #1327](https://bitbucket.org/osrf/gazebo/pull-request/1327)
+1. Fix ogre includes
+ * [Pull request #1323](https://bitbucket.org/osrf/gazebo/pull-request/1323)
+
+### Gazebo 4.1.0 (2014-11-20)
+
+1. Modified GUI rendering to improve the rendering update rate.
+ * [Pull request #1487](https://bitbucket.org/osrf/gazebo/pull-request/1487)
### Gazebo 4.1.0 (2014-11-20)
diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake
index bc03409..071cc95 100644
--- a/cmake/SearchForStuff.cmake
+++ b/cmake/SearchForStuff.cmake
@@ -114,7 +114,7 @@ if (PKG_CONFIG_FOUND)
#################################################
# Find DART
- find_package(DARTCore 4.1 QUIET)
+ find_package(DARTCore 4.3.3 QUIET)
if (DARTCore_FOUND)
message (STATUS "Looking for DARTCore - found")
set (HAVE_DART TRUE)
@@ -370,10 +370,13 @@ endif ()
########################################
# Find SDFormat
-find_package(SDFormat 2.1.0)
+find_package(SDFormat 2.3.1)
+if (NOT SDFormat_FOUND)
+ find_package(SDFormat 3)
+endif()
if (NOT SDFormat_FOUND)
message (STATUS "Looking for SDFormat - not found")
- BUILD_ERROR ("Missing: SDF version >=2.1.0. Required for reading and writing SDF files.")
+ BUILD_ERROR ("Missing: SDF version >=2.3.1. Required for reading and writing SDF files.")
else()
message (STATUS "Looking for SDFormat - found")
endif()
diff --git a/cmake/gazebo-config.cmake.in b/cmake/gazebo-config.cmake.in
index 25f3bea..1df9e4b 100644
--- a/cmake/gazebo-config.cmake.in
+++ b/cmake/gazebo-config.cmake.in
@@ -39,7 +39,7 @@ execute_process(COMMAND pkg-config --variable=prefix OGRE
# Add the OGRE cmake path to CMAKE_MODULE_PATH
set(CMAKE_MODULE_PATH
- "${OGRE_INSTALL_PREFIX}/share/OGRE/cmake/modules;${OGRE_INSTALL_PREFIX}/CMake;${CMAKE_MODULE_PATH}")
+ "${OGRE_INSTALL_PREFIX}/share/OGRE/cmake/modules;${OGRE_INSTALL_PREFIX}/lib/OGRE/cmake;${OGRE_INSTALL_PREFIX}/CMake;${CMAKE_MODULE_PATH}")
foreach(dep @PKG_DEPENDS@)
if(NOT ${dep}_FOUND)
diff --git a/cmake/setup.sh.in b/cmake/setup.sh.in
index e8d0765..ec472f6 100644
--- a/cmake/setup.sh.in
+++ b/cmake/setup.sh.in
@@ -1,7 +1,7 @@
export GAZEBO_MASTER_URI=http://@GAZEBO_DEFAULT_MASTER_HOST@:@GAZEBO_DEFAULT_MASTER_PORT@
export GAZEBO_MODEL_DATABASE_URI=@GAZEBO_MODEL_DATABASE_URI@
-export GAZEBO_RESOURCE_PATH=@GAZEBO_RESOURCE_PATH@
-export GAZEBO_PLUGIN_PATH=@GAZEBO_PLUGIN_PATH@
-export GAZEBO_MODEL_PATH=@GAZEBO_MODEL_PATH@
+export GAZEBO_RESOURCE_PATH=@GAZEBO_RESOURCE_PATH@:${GAZEBO_RESOURCE_PATH}
+export GAZEBO_PLUGIN_PATH=@GAZEBO_PLUGIN_PATH@:${GAZEBO_PLUGIN_PATH}
+export GAZEBO_MODEL_PATH=@GAZEBO_MODEL_PATH@:${GAZEBO_MODEL_PATH}
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:@GAZEBO_PLUGIN_PATH@
export OGRE_RESOURCE_PATH=@OGRE_RESOURCE_PATH@
diff --git a/deps/opende/src/step.cpp b/deps/opende/src/step.cpp
index 42fca21..4037e6e 100644
--- a/deps/opende/src/step.cpp
+++ b/deps/opende/src/step.cpp
@@ -456,6 +456,7 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
// force mixing vector `cfm', and LCP low and high bound vectors, and an
// 'findex' vector.
dReal *lo, *hi, *J, *A, *rhs;
+ dReal *c_v_max;
int *findex;
{
@@ -473,6 +474,10 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
findex = context->AllocateArray<int> (mlocal);
for (int i=0; i<mlocal; ++i) findex[i] = -1;
+
+ c_v_max = context->AllocateArray<dReal> (mlocal);
+ for(int i=0; i<mlocal; i++) c_v_max[i] = world->contactp.max_vel;
+
int mskip = dPAD(mlocal);
A = context->AllocateArray<dReal> (mlocal*mskip);
dSetZero (A,mlocal*mskip);
@@ -552,6 +557,8 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
Jinfo.lo = lo + ofsi;
Jinfo.hi = hi + ofsi;
Jinfo.findex = findex + ofsi;
+ Jinfo.c_v_max = c_v_max + ofsi;
+
#ifdef USE_JOINT_DAMPING
/*******************************************************/
@@ -962,6 +969,7 @@ size_t dxEstimateStepMemoryRequirements (dxBody * const * /*body*/, int nb, dxJo
sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * mskip * m); // for A
sub1_res2 += 3 * dEFFICIENT_SIZE(sizeof(dReal) * m); // for lo, hi, rhs
sub1_res2 += dEFFICIENT_SIZE(sizeof(int) * m); // for findex
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * m); // for c_v_max
#ifdef USE_JOINT_DAMPING
sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 12 * m_damp); // for J_damp
sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * m_damp); // for coeff_damp
diff --git a/doc/gazebo.in b/doc/gazebo.in
index da8f056..9c0bcc0 100644
--- a/doc/gazebo.in
+++ b/doc/gazebo.in
@@ -1219,7 +1219,7 @@ MATHJAX_EXTENSIONS =
# typically be disabled. For large projects the javascript based search engine
# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution.
-SEARCHENGINE = YES
+SEARCHENGINE = NO
# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
# implemented using a PHP enabled web server instead of at the web client
@@ -1663,7 +1663,7 @@ CLASS_GRAPH = YES
# indirect implementation dependencies (inheritance, containment, and
# class references variables) of the class with other documented classes.
-COLLABORATION_GRAPH = YES
+COLLABORATION_GRAPH = NO
# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen
# will generate a graph for groups, showing the direct groups dependencies
diff --git a/doc/header.html b/doc/header.html
index 53a706e..44e9cdb 100644
--- a/doc/header.html
+++ b/doc/header.html
@@ -55,6 +55,7 @@
</dl>
</div>
<div>
+ <!--
<div id="MSearchBox" class="MSearchBoxInactive">
<span>
<img id="MSearchSelect" src="search/mag_sel.png"
@@ -66,7 +67,7 @@
onblur="searchBox.OnSearchFieldFocus(false)"
onkeyup="searchBox.OnSearchFieldChange(event)"/>
</span>
- </div> <!-- End MSearchBox -->
+ </div> End MSearchBox -->
<div id="MSearchResultsWindow" style="position: static; display: block; border: none; background-color: #ffffff; width: 18em;">
<iframe src="javascript:void(0)" frameborder="0"
diff --git a/gazebo/common/AudioDecoder.cc b/gazebo/common/AudioDecoder.cc
index 4b251cd..96d167d 100644
--- a/gazebo/common/AudioDecoder.cc
+++ b/gazebo/common/AudioDecoder.cc
@@ -88,14 +88,14 @@ bool AudioDecoder::Decode(uint8_t **_outBuffer, unsigned int *_outBufferSize)
if (!decodedFrame)
{
- if (!(decodedFrame = avcodec_alloc_frame()))
+ if (!(decodedFrame = common::AVFrameAlloc()))
{
gzerr << "Audio decoder out of memory\n";
result = false;
}
}
else
- avcodec_get_frame_defaults(decodedFrame);
+ common::AVFrameUnref(decodedFrame);
av_init_packet(&packet);
while (av_read_frame(this->formatCtx, &packet) == 0)
diff --git a/gazebo/common/CMakeLists.txt b/gazebo/common/CMakeLists.txt
index 135d4e0..754638c 100644
--- a/gazebo/common/CMakeLists.txt
+++ b/gazebo/common/CMakeLists.txt
@@ -47,6 +47,7 @@ set (sources
Time.cc
Timer.cc
Video.cc
+ ffmpeg_inc.cc
)
set (headers
diff --git a/gazebo/common/Dem.cc b/gazebo/common/Dem.cc
index 19f9ede..33ac0d2 100644
--- a/gazebo/common/Dem.cc
+++ b/gazebo/common/Dem.cc
@@ -20,7 +20,10 @@
#include <gazebo/gazebo_config.h>
#ifdef HAVE_GDAL
-# include <gdal/ogr_spatialref.h>
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wfloat-equal"
+# include <ogr_spatialref.h>
+# pragma GCC diagnostic pop
#endif
#include "gazebo/common/CommonIface.hh"
diff --git a/gazebo/common/DemPrivate.hh b/gazebo/common/DemPrivate.hh
index a4833dd..3e6f974 100644
--- a/gazebo/common/DemPrivate.hh
+++ b/gazebo/common/DemPrivate.hh
@@ -22,7 +22,7 @@
#include <gazebo/util/system.hh>
#ifdef HAVE_GDAL
-# include <gdal/gdal_priv.h>
+# include <gdal_priv.h>
# include <vector>
namespace gazebo
diff --git a/gazebo/common/MeshManager.cc b/gazebo/common/MeshManager.cc
index 9a28485..97ef33d 100644
--- a/gazebo/common/MeshManager.cc
+++ b/gazebo/common/MeshManager.cc
@@ -262,18 +262,17 @@ void MeshManager::CreateSphere(const std::string &name, float radius,
mesh->AddSubMesh(subMesh);
// Generate the group of rings for the sphere
- for (ring = 0; ring <= rings; ring++)
+ for (ring = 0; ring <= rings; ++ring)
{
float r0 = radius * sinf(ring * deltaRingAngle);
vert.y = radius * cosf(ring * deltaRingAngle);
// Generate the group of segments for the current ring
- for (seg = 0; seg <= segments; seg++)
+ for (seg = 0; seg <= segments; ++seg)
{
vert.x = r0 * sinf(seg * deltaSegAngle);
vert.z = r0 * cosf(seg * deltaSegAngle);
- // TODO: Don't think these normals are correct.
norm = vert;
norm.Normalize();
@@ -298,8 +297,6 @@ void MeshManager::CreateSphere(const std::string &name, float radius,
}
}
}
-
- mesh->RecalculateNormals();
}
//////////////////////////////////////////////////
@@ -404,21 +401,25 @@ void MeshManager::CreateBox(const std::string &name, const math::Vector3 &sides,
// Vertex values
float v[8][3] =
{
- {-1, -1, -1}, {-1, -1, +1}, {+1, -1, +1}, {+1, -1, -1},
- {-1, +1, -1}, {-1, +1, +1}, {+1, +1, +1}, {+1, +1, -1}
+ {-1, -1, -1},
+ {-1, -1, +1},
+ {+1, -1, +1},
+ {+1, -1, -1},
+ {-1, +1, -1},
+ {-1, +1, +1},
+ {+1, +1, +1},
+ {+1, +1, -1}
};
- // Normals for each vertex
- float n[8][3]=
+ // Normals for each face
+ float n[6][3]=
{
- {-0.577350, -0.577350, -0.577350},
- {-0.577350, -0.577350, 0.577350},
- {0.577350, -0.577350, 0.577350},
- {0.577350, -0.577350, -0.577350},
- {-0.577350, 0.577350, -0.577350},
- {-0.577350, 0.577350, 0.577350},
- {0.577350, 0.577350, 0.577350},
- {0.577350, 0.577350, -0.577350}
+ {+0, -1, +0},
+ {+0, +1, +0},
+ {+0, +0, +1},
+ {-1, +0, +0},
+ {+0, +0, -1},
+ {+1, +0, +0},
};
// Texture coords
@@ -453,7 +454,7 @@ void MeshManager::CreateBox(const std::string &name, const math::Vector3 &sides,
};
// Compute the vertices
- for (i = 0; i < 8; i++)
+ for (i = 0; i < 8; ++i)
{
v[i][0] *= sides.x * 0.5;
v[i][1] *= sides.y * 0.5;
@@ -461,15 +462,15 @@ void MeshManager::CreateBox(const std::string &name, const math::Vector3 &sides,
}
// For each face
- for (i = 0; i < 6; i++)
+ for (i = 0; i < 6; ++i)
{
// For each vertex in the face
for (k = 0; k < 4; k++)
{
- subMesh->AddVertex(v[faces[i][k]][0], v[faces[i][k]][1],
- v[faces[i][k]][2]);
- subMesh->AddNormal(n[faces[i][k]][0], n[faces[i][k]][1],
- n[faces[i][k]][2]);
+ subMesh->AddVertex(v[faces[i][k]][0],
+ v[faces[i][k]][1],
+ v[faces[i][k]][2]);
+ subMesh->AddNormal(n[i][0], n[i][1], n[i][2]);
subMesh->AddTexCoord(t[k][0], t[k][1]);
}
}
@@ -477,8 +478,6 @@ void MeshManager::CreateBox(const std::string &name, const math::Vector3 &sides,
// Set the indices
for (i = 0; i < 36; i++)
subMesh->AddIndex(ind[i]);
-
- subMesh->RecalculateNormals();
}
//////////////////////////////////////////////////
@@ -708,7 +707,6 @@ void MeshManager::CreateCylinder(const std::string &name, float radius,
{
math::Vector3 vert, norm;
unsigned int verticeIndex = 0;
- unsigned int i, j;
int ring, seg;
float deltaSegAngle = (2.0 * M_PI / segments);
@@ -724,20 +722,20 @@ void MeshManager::CreateCylinder(const std::string &name, float radius,
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
-
- // Generate the group of rings for the sphere
- for (ring = 0; ring <= rings; ring++)
+ // Generate the group of rings for the cylinder
+ for (ring = 0; ring <= rings; ++ring)
{
vert.z = ring * height/rings - height/2.0;
// Generate the group of segments for the current ring
- for (seg = 0; seg <= segments; seg++)
+ for (seg = 0; seg <= segments; ++seg)
{
vert.y = radius * cosf(seg * deltaSegAngle);
vert.x = radius * sinf(seg * deltaSegAngle);
// TODO: Don't think these normals are correct.
norm = vert;
+ norm.z = 0;
norm.Normalize();
// Add one vertex to the strip which makes up the sphere
@@ -761,50 +759,63 @@ void MeshManager::CreateCylinder(const std::string &name, float radius,
}
}
- /// The top cap vertex
- subMesh->AddVertex(0, 0, height/2.0);
- subMesh->AddNormal(0, 0, 1);
- subMesh->AddTexCoord(0, 0);
-
- // The bottom cap vertex
- subMesh->AddVertex(0, 0, -height/2.0);
- subMesh->AddNormal(0, 0, -1);
- subMesh->AddTexCoord(0, 0);
-
- // Create the top fan
- verticeIndex += segments + 1;
- for (seg = 0; seg < segments; seg++)
+ // This block generates the top cap
{
- subMesh->AddIndex(verticeIndex);
- subMesh->AddIndex(verticeIndex - segments + seg);
- subMesh->AddIndex(verticeIndex - segments + seg - 1);
- }
+ vert.z = height/2.0;
+ // Generate the group of segments for the top ring
+ for (seg = 0; seg <= segments; ++seg)
+ {
+ vert.y = radius * cosf(seg * deltaSegAngle);
+ vert.x = radius * sinf(seg * deltaSegAngle);
+ subMesh->AddVertex(vert);
+ subMesh->AddNormal(0, 0, 1);
+ subMesh->AddTexCoord(
+ static_cast<float>(seg) / static_cast<float>(segments), 1.0);
+ }
- // Create the bottom fan
- verticeIndex++;
- for (seg = 0; seg < segments; seg++)
- {
- subMesh->AddIndex(verticeIndex);
- subMesh->AddIndex(seg);
- subMesh->AddIndex(seg+1);
+ // The top-middle cap vertex
+ subMesh->AddVertex(0, 0, height/2.0);
+ subMesh->AddNormal(0, 0, 1);
+ subMesh->AddTexCoord(0, 0);
+
+ // Create the top fan
+ verticeIndex = subMesh->GetVertexCount()-1;
+ for (seg = 0; seg < segments; seg++)
+ {
+ subMesh->AddIndex(verticeIndex);
+ subMesh->AddIndex(verticeIndex - segments + seg);
+ subMesh->AddIndex(verticeIndex - segments + seg - 1);
+ }
}
- // Fix all the normals
- for (i = 0; i+3 < subMesh->GetIndexCount(); i+= 3)
+ // This block generates the bottom cap
{
- norm.Set();
-
- for (j = 0; j < 3; j++)
- norm += subMesh->GetNormal(subMesh->GetIndex(i+j));
+ vert.z = -height/2.0;
+ // Generate the group of segments for the bottom ring
+ for (seg = 0; seg <= segments; ++seg)
+ {
+ vert.y = radius * cosf(seg * deltaSegAngle);
+ vert.x = radius * sinf(seg * deltaSegAngle);
+ subMesh->AddVertex(vert);
+ subMesh->AddNormal(0, 0, -1);
+ subMesh->AddTexCoord(
+ static_cast<float>(seg) / static_cast<float>(segments), 0.0);
+ }
- norm /= 3;
- norm.Normalize();
+ // The bottom-middle cap vertex
+ subMesh->AddVertex(0, 0, -height/2.0);
+ subMesh->AddNormal(0, 0, -1);
+ subMesh->AddTexCoord(0, 0);
- for (j = 0; j < 3; j++)
- subMesh->SetNormal(subMesh->GetIndex(i+j), norm);
+ // Create the bottom fan
+ verticeIndex = subMesh->GetVertexCount()-1;
+ for (seg = 0; seg < segments; seg++)
+ {
+ subMesh->AddIndex(verticeIndex);
+ subMesh->AddIndex(verticeIndex - segments + seg - 1);
+ subMesh->AddIndex(verticeIndex - segments + seg);
+ }
}
-
- mesh->RecalculateNormals();
}
//////////////////////////////////////////////////
diff --git a/gazebo/common/Video.cc b/gazebo/common/Video.cc
index 8a0bab1..61aaec9 100644
--- a/gazebo/common/Video.cc
+++ b/gazebo/common/Video.cc
@@ -92,7 +92,7 @@ bool Video::Load(const std::string &_filename)
if (this->formatCtx || this->avFrame || this->codecCtx)
this->Cleanup();
- this->avFrame = avcodec_alloc_frame();
+ this->avFrame = common::AVFrameAlloc();
// Open video file
if (avformat_open_input(&this->formatCtx, _filename.c_str(), NULL, NULL) < 0)
diff --git a/gazebo/common/ffmpeg_inc.h b/gazebo/common/ffmpeg_inc.cc
similarity index 50%
copy from gazebo/common/ffmpeg_inc.h
copy to gazebo/common/ffmpeg_inc.cc
index 8933ef0..4135320 100644
--- a/gazebo/common/ffmpeg_inc.h
+++ b/gazebo/common/ffmpeg_inc.cc
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2012-2015 Open Source Robotics Foundation
+ * Copyright (C) 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.
@@ -15,23 +15,30 @@
*
*/
-#ifndef _GAZEBO_FFMPEG_INC_HH_
-#define _GAZEBO_FFMPEG_INC_HH_
-
-#pragma GCC system_header
+#include "gazebo/common/ffmpeg_inc.h"
#ifdef HAVE_FFMPEG
-#ifndef INT64_C
-#define INT64_C(c) (c ## LL)
-#define UINT64_C(c) (c ## ULL)
+using namespace gazebo;
+
+//////////////////////////////////////////////////
+AVFrame *common::AVFrameAlloc(void)
+{
+#if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(55, 28, 1)
+ return av_frame_alloc();
+#else
+ return avcodec_alloc_frame();
#endif
+}
-extern "C" {
-#include <libavcodec/avcodec.h>
-#include <libavformat/avformat.h>
-#include <libavutil/opt.h>
-#include <libswscale/swscale.h>
+//////////////////////////////////////////////////
+void common::AVFrameUnref(AVFrame *_frame)
+{
+#if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(55, 28, 1)
+ av_frame_unref(_frame);
+#else
+ avcodec_get_frame_defaults(_frame);
+#endif
}
-#endif // ifdef HAVE_FFMPEG
-#endif // ifndef _GAZEBO_FFMPEG_INC_HH
+// ifdef HAVE_FFMPEG
+#endif
diff --git a/gazebo/common/ffmpeg_inc.h b/gazebo/common/ffmpeg_inc.h
index 8933ef0..3d0fea2 100644
--- a/gazebo/common/ffmpeg_inc.h
+++ b/gazebo/common/ffmpeg_inc.h
@@ -18,6 +18,8 @@
#ifndef _GAZEBO_FFMPEG_INC_HH_
#define _GAZEBO_FFMPEG_INC_HH_
+#include <gazebo/gazebo_config.h>
+
#pragma GCC system_header
#ifdef HAVE_FFMPEG
@@ -32,6 +34,21 @@ extern "C" {
#include <libavutil/opt.h>
#include <libswscale/swscale.h>
}
-#endif // ifdef HAVE_FFMPEG
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace common
+ {
+ /// \brief Helper function to avoid deprecation warnings.
+ GAZEBO_VISIBLE
+ AVFrame *AVFrameAlloc(void);
+
+ /// \brief Helper function to avoid deprecation warnings.
+ GAZEBO_VISIBLE
+ void AVFrameUnref(AVFrame *_frame);
+ }
+}
+#endif // ifdef HAVE_FFMPEG
#endif // ifndef _GAZEBO_FFMPEG_INC_HH
diff --git a/gazebo/gui/BoxMaker.cc b/gazebo/gui/BoxMaker.cc
index 76c2519..aedec37 100644
--- a/gazebo/gui/BoxMaker.cc
+++ b/gazebo/gui/BoxMaker.cc
@@ -177,39 +177,22 @@ void BoxMaker::OnMouseDrag(const common::MouseEvent &_event)
/////////////////////////////////////////////////
std::string BoxMaker::GetSDFString()
{
- std::ostringstream newModelStr;
- newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
- << "<model name='unit_box_" << counter << "'>"
- << "<pose>0 0 0.5 0 0 0</pose>"
- << "<link name ='link'>"
- << "<inertial><mass>1.0</mass></inertial>"
- << "<collision name ='collision'>"
- << "<geometry>"
- << "<box>"
- << "<size>1.0 1.0 1.0</size>"
- << "</box>"
- << "</geometry>"
- << "</collision>"
- << "<visual name ='visual'>"
- << "<geometry>"
- << "<box>"
- << "<size>1.0 1.0 1.0</size>"
- << "</box>"
- << "</geometry>"
- << "<material>"
- << "<script>"
- << "<uri>file://media/materials/scripts/gazebo.material</uri>"
- << "<name>Gazebo/Grey</name>"
- << "</script>"
- << "</material>"
- << "</visual>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- return newModelStr.str();
+ msgs::Model model;
+ {
+ std::ostringstream modelName;
+ modelName << "unit_box_" << counter;
+ model.set_name(modelName.str());
+ }
+ msgs::Set(model.mutable_pose(), math::Pose(0, 0, 0.5, 0, 0, 0));
+ msgs::AddBoxLink(model, 1.0, math::Vector3::One);
+ model.mutable_link(0)->set_name("link");
+
+ return "<sdf version='" + std::string(SDF_VERSION) + "'>"
+ + msgs::ModelToSDF(model)->ToString("")
+ + "</sdf>";
}
+/////////////////////////////////////////////////
void BoxMaker::CreateTheEntity()
{
msgs::Factory msg;
diff --git a/gazebo/gui/CMakeLists.txt b/gazebo/gui/CMakeLists.txt
index 5e9a6fd..8001b7a 100644
--- a/gazebo/gui/CMakeLists.txt
+++ b/gazebo/gui/CMakeLists.txt
@@ -144,11 +144,11 @@ set (resources resources.qrc)
QT4_WRAP_CPP(headers_MOC ${qt_headers})
QT4_ADD_RESOURCES(resources_RCC ${resources})
-gz_add_executable(gzclient ${sources} main.cc ${headers_MOC} ${headers} ${resources_RCC})
gz_add_library(gazebo_gui ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+gz_add_executable(gzclient main.cc)
add_dependencies(gazebo_gui gazebo_msgs)
-add_dependencies(gzclient gazebo_msgs)
+add_dependencies(gzclient gazebo_gui)
target_link_libraries(gazebo_gui
gazebo_common
@@ -174,6 +174,7 @@ target_link_libraries(gzclient
gazebo_rendering
gazebo_sensors
gazebo_msgs
+ gazebo_gui
gazebo_gui_building
gazebo_gui_model
gazebo_gui_terrain
diff --git a/gazebo/gui/CylinderMaker.cc b/gazebo/gui/CylinderMaker.cc
index 3f19e07..ec45f5f 100644
--- a/gazebo/gui/CylinderMaker.cc
+++ b/gazebo/gui/CylinderMaker.cc
@@ -186,41 +186,19 @@ void CylinderMaker::OnMouseDrag(const common::MouseEvent &_event)
/////////////////////////////////////////////////
std::string CylinderMaker::GetSDFString()
{
- std::ostringstream newModelStr;
-
- newModelStr
- << "<sdf version ='" << SDF_VERSION << "'>"
- << " <model name ='unit_cylinder_" << counter << "'>"
- << " <pose>0 0 0.5 0 0 0</pose>"
- << " <link name='link'>"
- << " <inertial><mass>1.0</mass></inertial>"
- << " <collision name='collision'>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>0.5</radius>"
- << " <length>1.0</length>"
- << " </cylinder>"
- << " </geometry>"
- << " </collision>"
- << " <visual name='visual'>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>0.5</radius>"
- << " <length>1.0</length>"
- << " </cylinder>"
- << " </geometry>"
- << " <material>"
- << " <script>"
- << " <uri>file://media/materials/scripts/gazebo.material</uri>"
- << " <name>Gazebo/Grey</name>"
- << " </script>"
- << " </material>"
- << " </visual>"
- << " </link>"
- << " </model>"
- << "</sdf>";
-
- return newModelStr.str();
+ msgs::Model model;
+ {
+ std::ostringstream modelName;
+ modelName << "unit_cylinder_" << counter;
+ model.set_name(modelName.str());
+ }
+ msgs::Set(model.mutable_pose(), math::Pose(0, 0, 0.5, 0, 0, 0));
+ msgs::AddCylinderLink(model, 1.0, 0.5, 1.0);
+ model.mutable_link(0)->set_name("link");
+
+ return "<sdf version='" + std::string(SDF_VERSION) + "'>"
+ + msgs::ModelToSDF(model)->ToString("")
+ + "</sdf>";
}
/////////////////////////////////////////////////
diff --git a/gazebo/gui/GLWidget.cc b/gazebo/gui/GLWidget.cc
index 09df11d..46d8c5b 100644
--- a/gazebo/gui/GLWidget.cc
+++ b/gazebo/gui/GLWidget.cc
@@ -65,8 +65,8 @@ GLWidget::GLWidget(QWidget *_parent)
this->windowId = -1;
- setAttribute(Qt::WA_OpaquePaintEvent, true);
- setAttribute(Qt::WA_PaintOnScreen, true);
+ this->setAttribute(Qt::WA_OpaquePaintEvent, true);
+ this->setAttribute(Qt::WA_PaintOnScreen, true);
this->renderFrame = new QFrame;
this->renderFrame->setFrameShape(QFrame::NoFrame);
@@ -244,7 +244,12 @@ void GLWidget::paintEvent(QPaintEvent *_e)
event::Events::postRender();
}
+ else
+ {
+ event::Events::preRender();
+ }
+ this->update();
_e->accept();
}
diff --git a/gazebo/gui/MainWindow_TEST.cc b/gazebo/gui/MainWindow_TEST.cc
index ca3f421..487ca9d 100644
--- a/gazebo/gui/MainWindow_TEST.cc
+++ b/gazebo/gui/MainWindow_TEST.cc
@@ -34,6 +34,130 @@ void OnRequest(ConstRequestPtr &_msg)
g_gotSetWireframe = true;
}
+
+/////////////////////////////////////////////////
+void MainWindow_TEST::Selection()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/shapes.world", false, false, true);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ // get model at center of window - should get the ground plane
+ gazebo::rendering::VisualPtr vis =
+ cam->GetVisual(gazebo::math::Vector2i(0, 0));
+ QVERIFY(vis != NULL);
+ QVERIFY(vis->IsPlane());
+
+ // move camera to look at the box
+ gazebo::math::Pose cameraPose(gazebo::math::Vector3(-1, 0, 0.5),
+ gazebo::math::Vector3(0, 0, 0));
+ cam->SetWorldPose(cameraPose);
+ QVERIFY(cam->GetWorldPose() == cameraPose);
+
+ // verify we get a box
+ gazebo::rendering::VisualPtr vis2 =
+ cam->GetVisual(gazebo::math::Vector2i(0, 0));
+ QVERIFY(vis2 != NULL);
+ QVERIFY(vis2->GetRootVisual()->GetName() == "box");
+
+ // look upwards
+ gazebo::math::Quaternion pitch90(gazebo::math::Vector3(0, -1.57, 0));
+ cam->SetWorldRotation(pitch90);
+ QVERIFY(cam->GetWorldRotation() == pitch90);
+
+ // verify there is nothing in the middle of the window
+ gazebo::rendering::VisualPtr vis3 =
+ cam->GetVisual(gazebo::math::Vector2i(0, 0));
+ QVERIFY(vis3 == NULL);
+
+ // reset orientation
+ gazebo::math::Quaternion identityRot(gazebo::math::Vector3(0, 0, 0));
+ cam->SetWorldRotation(identityRot);
+ QVERIFY(cam->GetWorldRotation() == identityRot);
+
+ // verify we can still get the box
+ gazebo::rendering::VisualPtr vis4 =
+ cam->GetVisual(gazebo::math::Vector2i(0, 0));
+ QVERIFY(vis4 != NULL);
+ QVERIFY(vis4->GetRootVisual()->GetName() == "box");
+
+ // hide the box
+ vis4->SetVisible(false);
+ gazebo::rendering::VisualPtr vis5 =
+ cam->GetVisual(gazebo::math::Vector2i(0, 0));
+
+ // verify we don't get anything now
+ QVERIFY(vis5 == NULL);
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void MainWindow_TEST::UserCameraFPS()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/shapes.world", false, false, true);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ // Wait a little bit for the average FPS to even out.
+ for (unsigned int i = 0; i < 10000; ++i)
+ {
+ gazebo::common::Time::NSleep(500000);
+ QCoreApplication::processEvents();
+ }
+
+ std::cerr << "\nFPS[" << cam->GetAvgFPS() << "]\n" << std::endl;
+
+ QVERIFY(cam->GetAvgFPS() > 55.0);
+ QVERIFY(cam->GetAvgFPS() < 75.0);
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
/////////////////////////////////////////////////
void MainWindow_TEST::CopyPaste()
{
diff --git a/gazebo/gui/MainWindow_TEST.hh b/gazebo/gui/MainWindow_TEST.hh
index 9b2e135..232ef8f 100644
--- a/gazebo/gui/MainWindow_TEST.hh
+++ b/gazebo/gui/MainWindow_TEST.hh
@@ -25,6 +25,12 @@ class MainWindow_TEST : public QTestFixture
{
Q_OBJECT
+ /// \brief Test user camera entity selection
+ private slots: void Selection();
+
+ /// \brief Test user camera frames per second
+ private slots: void UserCameraFPS();
+
/// \brief Test copying and pasting a model and a light
private slots: void CopyPaste();
diff --git a/gazebo/gui/ModelManipulator.cc b/gazebo/gui/ModelManipulator.cc
index 4be1d17..b877e21 100644
--- a/gazebo/gui/ModelManipulator.cc
+++ b/gazebo/gui/ModelManipulator.cc
@@ -473,11 +473,16 @@ void ModelManipulator::OnMousePressEvent(const common::MouseEvent &_event)
// select model
vis = rootVis;
}
- else if (vis->GetParent() != rootVis)
+ else if (vis->GetParent() != rootVis &&
+ vis->GetParent() != this->dataPtr->scene->GetWorldVisual())
{
// select link
vis = vis->GetParent();
}
+ else
+ {
+ // select light
+ }
this->dataPtr->mouseMoveVisStartPose = vis->GetWorldPose();
diff --git a/gazebo/gui/RenderWidget.cc b/gazebo/gui/RenderWidget.cc
index e573793..22e898e 100644
--- a/gazebo/gui/RenderWidget.cc
+++ b/gazebo/gui/RenderWidget.cc
@@ -166,10 +166,6 @@ RenderWidget::RenderWidget(QWidget *_parent)
this->setLayout(mainLayout);
this->layout()->setContentsMargins(0, 0, 0, 0);
- this->timer = new QTimer(this);
- connect(this->timer, SIGNAL(timeout()), this, SLOT(update()));
- this->timer->start(44);
-
this->connections.push_back(
gui::Events::ConnectFollow(
boost::bind(&RenderWidget::OnFollow, this, _1)));
diff --git a/gazebo/gui/SphereMaker.cc b/gazebo/gui/SphereMaker.cc
index 370bb8b..2afcc5c 100644
--- a/gazebo/gui/SphereMaker.cc
+++ b/gazebo/gui/SphereMaker.cc
@@ -159,33 +159,19 @@ void SphereMaker::OnMouseDrag(const common::MouseEvent &_event)
/////////////////////////////////////////////////
std::string SphereMaker::GetSDFString()
{
- std::ostringstream newModelStr;
- newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
- << "<model name='unit_sphere_" << counter << "'>"
- << " <pose>0 0 0.5 0 0 0</pose>"
- << " <link name='link'>"
- << " <inertial><mass>1.0</mass></inertial>"
- << " <collision name='collision'>"
- << " <geometry>"
- << " <sphere><radius>0.5</radius></sphere>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <sphere><radius>0.5</radius></sphere>"
- << " </geometry>"
- << " <material>"
- << " <script>"
- << " <uri>file://media/materials/scripts/gazebo.material</uri>"
- << " <name>Gazebo/Grey</name>"
- << " </script>"
- << " </material>"
- << " </visual>"
- << " </link>"
- << " </model>"
- << "</sdf>";
-
- return newModelStr.str();
+ msgs::Model model;
+ {
+ std::ostringstream modelName;
+ modelName << "unit_sphere_" << counter;
+ model.set_name(modelName.str());
+ }
+ msgs::Set(model.mutable_pose(), math::Pose(0, 0, 0.5, 0, 0, 0));
+ msgs::AddSphereLink(model, 1.0, 0.5);
+ model.mutable_link(0)->set_name("link");
+
+ return "<sdf version='" + std::string(SDF_VERSION) + "'>"
+ + msgs::ModelToSDF(model)->ToString("")
+ + "</sdf>";
}
diff --git a/gazebo/math/Quaternion.hh b/gazebo/math/Quaternion.hh
index 003a8ae..b5feab9 100644
--- a/gazebo/math/Quaternion.hh
+++ b/gazebo/math/Quaternion.hh
@@ -68,7 +68,7 @@ namespace gazebo
public: Quaternion(const Vector3 &_rpy);
/// \brief Copy constructor
- /// \param qt Quaternion to copy
+ /// \param[in] _qt Quaternion to copy
public: Quaternion(const Quaternion &_qt);
/// \brief Destructor
@@ -82,7 +82,7 @@ namespace gazebo
public: void Invert();
/// \brief Get the inverse of this quaternion
- /// \return Inverse quarenion
+ /// \return Inverse quaternion
public: inline Quaternion GetInverse() const
{
double s = 0;
@@ -110,7 +110,7 @@ namespace gazebo
return q;
}
- /// \brief Set the quatern to the identity
+ /// \brief Set the quaternion to the identity
public: void SetToIdentity();
/// \brief Return the logarithm
@@ -144,42 +144,46 @@ namespace gazebo
public: void Set(double _u, double _x, double _y, double _z);
/// \brief Set the quaternion from Euler angles. The order of operations
- /// are roll, pitch, yaw.
- /// \param[in] vec Euler angle
+ /// is roll, pitch, yaw around a fixed body frame axis
+ /// (the original frame of the object before rotation is applied).
+ /// Roll is a rotation about x, pitch is about y, yaw is about z.
+ /// \param[in] _vec Euler angle
public: void SetFromEuler(const Vector3 &_vec);
/// \brief Set the quaternion from Euler angles.
/// \param[in] _roll Roll angle (radians).
- /// \param[in] _pitch Roll angle (radians).
- /// \param[in] _yaw Roll angle (radians).
+ /// \param[in] _pitch Pitch angle (radians).
+ /// \param[in] _yaw Yaw angle (radians).
public: void SetFromEuler(double _roll, double _pitch, double _yaw);
/// \brief Return the rotation in Euler angles
/// \return This quaternion as an Euler vector
public: Vector3 GetAsEuler() const;
- /// \brief Convert euler angles to quatern.
- /// \param[in]
+ /// \brief Convert euler angles to a quaternion.
+ /// \param[in] _vec The vector of angles to convert.
+ /// \return The converted quaternion.
public: static Quaternion EulerToQuaternion(const Vector3 &_vec);
/// \brief Convert euler angles to quatern.
/// \param[in] _x rotation along x
/// \param[in] _y rotation along y
/// \param[in] _z rotation along z
+ /// \return The converted quaternion.
public: static Quaternion EulerToQuaternion(double _x,
double _y,
double _z);
/// \brief Get the Euler roll angle in radians
- /// \return the roll
+ /// \return the roll component
public: double GetRoll();
/// \brief Get the Euler pitch angle in radians
- /// \return the pitch
+ /// \return the pitch component
public: double GetPitch();
/// \brief Get the Euler yaw angle in radians
- /// \return the yaw
+ /// \return the yaw component
public: double GetYaw();
/// \brief Return rotation as axis and angle
@@ -201,18 +205,18 @@ namespace gazebo
/// \return this quaternion + qt
public: Quaternion operator+=(const Quaternion &_qt);
- /// \brief Substraction operator
- /// \param[in] _qt quaternion to substract
+ /// \brief Subtraction operator
+ /// \param[in] _qt quaternion to subtract
/// \return this quaternion - _qt
public: Quaternion operator-(const Quaternion &_qt) const;
- /// \brief Substraction operator
- /// \param[in] _qt Quaternion for substraction
- /// \return This quatern - qt
+ /// \brief Subtraction operator
+ /// \param[in] _qt Quaternion for subtraction
+ /// \return This quaternion - qt
public: Quaternion operator-=(const Quaternion &_qt);
/// \brief Multiplication operator
- /// \param[in] _qt Quaternion for multiplication
+ /// \param[in] _q Quaternion for multiplication
/// \return This quaternion multiplied by the parameter
public: inline Quaternion operator*(const Quaternion &_q) const
{
@@ -223,18 +227,19 @@ namespace gazebo
this->w*_q.z + this->x*_q.y - this->y*_q.x + this->z*_q.w);
}
- /// \brief Multiplication operator
+ /// \brief Multiplication operator by a scalar.
/// \param[in] _f factor
- /// \return quaternion multiplied by _f
+ /// \return quaternion multiplied by the scalar
public: Quaternion operator*(const double &_f) const;
/// \brief Multiplication operator
/// \param[in] _qt Quaternion for multiplication
- /// \return This quatern multiplied by the parameter
+ /// \return This quaternion multiplied by the parameter
public: Quaternion operator*=(const Quaternion &qt);
/// \brief Vector3 multiplication operator
/// \param[in] _v vector to multiply
+ /// \return The result of the vector multiplication
public: Vector3 operator*(const Vector3 &_v) const;
/// \brief Equal to operator
@@ -263,14 +268,14 @@ namespace gazebo
/// \brief Do the reverse rotation of a vector by this quaternion
/// \param[in] _vec the vector
- /// \return the
+ /// \return the reversed vector
public: Vector3 RotateVectorReverse(Vector3 _vec) const;
- /// \brief See if a quatern is finite (e.g., not nan)
- /// \return True if quatern is finite
+ /// \brief See if a quaternion is finite (e.g., not nan)
+ /// \return True if quaternion is finite
public: bool IsFinite() const;
- /// \brief Correct any nan
+ /// \brief Correct any nan values in this quaternion
public: inline void Correct()
{
if (!std::isfinite(this->x))
@@ -292,6 +297,7 @@ namespace gazebo
}
/// \brief Get the quaternion as a 3x3 matrix
+ /// \return The 3x3 matrix form of the quaternion
public: Matrix3 GetAsMatrix3() const;
/// \brief Get the quaternion as a 4x4 matrix
@@ -299,15 +305,15 @@ namespace gazebo
public: Matrix4 GetAsMatrix4() const;
/// \brief Return the X axis
- /// \return the vector
+ /// \return the X axis of the vector
public: Vector3 GetXAxis() const;
/// \brief Return the Y axis
- /// \return the vector
+ /// \return the Y axis of the vector
public: Vector3 GetYAxis() const;
/// \brief Return the Z axis
- /// \return the vector
+ /// \return the Z axis of the vector
public: Vector3 GetZAxis() const;
/// \brief Round all values to _precision decimal places
@@ -328,6 +334,7 @@ namespace gazebo
/// \param[in] _rkQ the end quaternion
/// \param[in] _shortestPath when true, the rotation may be inverted to
/// get to minimize rotation
+ /// \return The result of the quadratic interpolation
public: static Quaternion Squad(double _fT, const Quaternion &_rkP,
const Quaternion &_rkA, const Quaternion &_rkB,
const Quaternion &_rkQ, bool _shortestPath = false);
@@ -339,6 +346,7 @@ namespace gazebo
/// \param[in] _rkQ the end quaternion
/// \param[in] _shortestPath when true, the rotation may be inverted to
/// get to minimize rotation
+ /// \return The result of the linear interpolation
public: static Quaternion Slerp(double _fT, const Quaternion &_rkP,
const Quaternion &_rkQ, bool _shortestPath = false);
@@ -351,16 +359,16 @@ namespace gazebo
public: Quaternion Integrate(const Vector3 &_angularVelocity,
const double _deltaT) const;
- /// \brief Attributes of the quaternion
+ /// \brief w value of the quaternion
public: double w;
- /// \brief Attributes of the quaternion
+ /// \brief x value of the quaternion
public: double x;
- /// \brief Attributes of the quaternion
+ /// \brief y value of the quaternion
public: double y;
- /// \brief Attributes of the quaternion
+ /// \brief z value of the quaternion
public: double z;
/// \brief Stream insertion operator
@@ -398,4 +406,3 @@ namespace gazebo
}
}
#endif
-
diff --git a/gazebo/math/Quaternion_TEST.cc b/gazebo/math/Quaternion_TEST.cc
index 4b623be..4652ae7 100644
--- a/gazebo/math/Quaternion_TEST.cc
+++ b/gazebo/math/Quaternion_TEST.cc
@@ -205,6 +205,29 @@ TEST_F(QuaternionTest, Quaternion)
EXPECT_TRUE(q.GetInverse().GetZAxis() == math::Vector3(0, 0, 1));
}
+ // Test RPY fixed-body-frame convention:
+ // Rotate each unit vector in roll and pitch
+ {
+ q = math::Quaternion(M_PI/2.0, M_PI/2.0, 0);
+ math::Vector3 v1(1, 0, 0);
+ math::Vector3 r1 = q.RotateVector(v1);
+ // 90 degrees about X does nothing,
+ // 90 degrees about Y sends point down to -Z
+ EXPECT_EQ(r1, math::Vector3(0, 0, -1));
+
+ math::Vector3 v2(0, 1, 0);
+ math::Vector3 r2 = q.RotateVector(v2);
+ // 90 degrees about X sends point to +Z
+ // 90 degrees about Y sends point to +X
+ EXPECT_EQ(r2, math::Vector3(1, 0, 0));
+
+ math::Vector3 v3(0, 0, 1);
+ math::Vector3 r3 = q.RotateVector(v3);
+ // 90 degrees about X sends point to -Y
+ // 90 degrees about Y does nothing
+ EXPECT_EQ(r3, math::Vector3(0, -1, 0));
+ }
+
{
// now try a harder case (axis[1,2,3], rotation[0.3*pi])
// verified with octave
@@ -285,6 +308,70 @@ TEST_F(QuaternionTest, Integrate)
EXPECT_EQ(qYaw.GetAsEuler(), math::Vector3::UnitZ);
}
+ // Integrate sequentially along single axes in order XYZ,
+ // expect rotations to match Euler Angles
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qX = q.Integrate(math::Vector3::UnitX, angle);
+ math::Quaternion qXY = qX.Integrate(math::Vector3::UnitY, angle);
+ EXPECT_EQ(qXY.GetAsEuler(), angle*math::Vector3(1, 1, 0));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qX = q.Integrate(math::Vector3::UnitX, angle);
+ math::Quaternion qXZ = qX.Integrate(math::Vector3::UnitZ, angle);
+ EXPECT_EQ(qXZ.GetAsEuler(), angle*math::Vector3(1, 0, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qY = q.Integrate(math::Vector3::UnitY, angle);
+ math::Quaternion qYZ = qY.Integrate(math::Vector3::UnitZ, angle);
+ EXPECT_EQ(qYZ.GetAsEuler(), angle*math::Vector3(0, 1, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qX = q.Integrate(math::Vector3::UnitX, angle);
+ math::Quaternion qXY = qX.Integrate(math::Vector3::UnitY, angle);
+ math::Quaternion qXYZ = qXY.Integrate(math::Vector3::UnitZ, angle);
+ EXPECT_EQ(qXYZ.GetAsEuler(), angle*math::Vector3::One);
+ }
+
+ // Integrate sequentially along single axes in order ZYX,
+ // expect rotations to not match Euler Angles
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, angle);
+ math::Quaternion qZY = qZ.Integrate(math::Vector3::UnitY, angle);
+ EXPECT_NE(qZY.GetAsEuler(), angle*math::Vector3(0, 1, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, angle);
+ math::Quaternion qZX = qZ.Integrate(math::Vector3::UnitX, angle);
+ EXPECT_NE(qZX.GetAsEuler(), angle*math::Vector3(1, 0, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, angle);
+ math::Quaternion qZY = qZ.Integrate(math::Vector3::UnitY, angle);
+ math::Quaternion qZYX = qZY.Integrate(math::Vector3::UnitX, angle);
+ EXPECT_NE(qZYX.GetAsEuler(), angle*math::Vector3(1, 1, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qY = q.Integrate(math::Vector3::UnitY, angle);
+ math::Quaternion qYX = qY.Integrate(math::Vector3::UnitX, angle);
+ EXPECT_NE(qYX.GetAsEuler(), angle*math::Vector3(1, 1, 0));
+ }
+
// Integrate a full rotation about different axes,
// expect no change.
{
diff --git a/gazebo/math/SignalStats.cc b/gazebo/math/SignalStats.cc
index c9336b0..113a93f 100644
--- a/gazebo/math/SignalStats.cc
+++ b/gazebo/math/SignalStats.cc
@@ -144,10 +144,9 @@ size_t SignalStats::Count() const
std::map<std::string, double> SignalStats::Map() const
{
std::map<std::string, double> map;
- for (SignalStatistic_V::const_iterator iter = this->dataPtr->stats.begin();
- iter != this->dataPtr->stats.end(); ++iter)
+ for (auto const &statistic : this->dataPtr->stats)
{
- map[(*iter)->ShortName()] = (*iter)->Value();
+ map[statistic->ShortName()] = statistic->Value();
}
return map;
}
@@ -155,10 +154,9 @@ std::map<std::string, double> SignalStats::Map() const
//////////////////////////////////////////////////
void SignalStats::InsertData(const double _data)
{
- for (SignalStatistic_V::iterator iter = this->dataPtr->stats.begin();
- iter != this->dataPtr->stats.end(); ++iter)
+ for (auto &statistic : this->dataPtr->stats)
{
- (*iter)->InsertData(_data);
+ statistic->InsertData(_data);
}
}
@@ -220,10 +218,9 @@ bool SignalStats::InsertStatistics(const std::string &_names)
bool result = true;
std::vector<std::string> names;
boost::split(names, _names, boost::is_any_of(","));
- for (std::vector<std::string>::iterator iter = names.begin();
- iter != names.end(); ++iter)
+ for (auto &statistic : names)
{
- result = result && this->InsertStatistic(*iter);
+ result = result && this->InsertStatistic(statistic);
}
return result;
}
@@ -231,10 +228,9 @@ bool SignalStats::InsertStatistics(const std::string &_names)
//////////////////////////////////////////////////
void SignalStats::Reset()
{
- for (SignalStatistic_V::iterator iter = this->dataPtr->stats.begin();
- iter != this->dataPtr->stats.end(); ++iter)
+ for (auto &statistic : this->dataPtr->stats)
{
- (*iter)->Reset();
+ statistic->Reset();
}
}
diff --git a/gazebo/msgs/msgs.cc b/gazebo/msgs/msgs.cc
index b899614..8892504 100644
--- a/gazebo/msgs/msgs.cc
+++ b/gazebo/msgs/msgs.cc
@@ -34,6 +34,13 @@ namespace gazebo
{
namespace msgs
{
+ /// \internal
+ /// \brief Internal function to create an SDF element from msgs::Axis.
+ /// It is only intended to be used by JointToSDF.
+ /// \param[in] _msg The msgs::Axis object.
+ /// \param[in] _sdf sdf::ElementPtr to fill with data.
+ void AxisToSDF(const msgs::Axis &_msg, sdf::ElementPtr _sdf);
+
/// Create a request message
msgs::Request *CreateRequest(const std::string &_request,
const std::string &_data)
@@ -315,6 +322,13 @@ namespace gazebo
{
result = msgs::Joint::GEARBOX;
}
+ else
+ {
+ gzerr << "Unrecognized JointType ["
+ << _str
+ << "], returning REVOLUTE"
+ << std::endl;
+ }
return result;
}
@@ -361,6 +375,8 @@ namespace gazebo
default:
{
result = "unknown";
+ gzerr << "Unrecognized JointType [" << _type << "]"
+ << std::endl;
break;
}
}
@@ -710,7 +726,7 @@ namespace gazebo
// Load the geometry
if (_sdf->HasElement("geometry"))
{
- msgs::Geometry *geomMsg = result.mutable_geometry();
+ auto geomMsg = result.mutable_geometry();
geomMsg->CopyFrom(GeometryFromSDF(_sdf->GetElement("geometry")));
}
@@ -718,7 +734,7 @@ namespace gazebo
if (_sdf->HasElement("material"))
{
sdf::ElementPtr elem = _sdf->GetElement("material");
- msgs::Material *matMsg = result.mutable_material();
+ auto matMsg = result.mutable_material();
if (elem->HasElement("script"))
{
@@ -806,6 +822,190 @@ namespace gazebo
}
/////////////////////////////////////////////////
+ sdf::ElementPtr VisualToSDF(const msgs::Visual &_msg,
+ sdf::ElementPtr _sdf)
+ {
+ sdf::ElementPtr visualSDF;
+
+ if (_sdf)
+ {
+ visualSDF = _sdf;
+ }
+ else
+ {
+ visualSDF.reset(new sdf::Element);
+ sdf::initFile("visual.sdf", visualSDF);
+ }
+
+ if (_msg.has_name())
+ visualSDF->GetAttribute("name")->Set(_msg.name());
+
+ if (_msg.has_cast_shadows())
+ visualSDF->GetElement("cast_shadows")->Set(_msg.cast_shadows());
+
+ if (_msg.has_transparency())
+ visualSDF->GetElement("transparency")->Set(_msg.transparency());
+
+ if (_msg.has_laser_retro())
+ visualSDF->GetElement("laser_retro")->Set(_msg.laser_retro());
+
+ if (_msg.has_pose())
+ visualSDF->GetElement("pose")->Set(msgs::Convert(_msg.pose()));
+
+ // Load the geometry
+ if (_msg.has_geometry())
+ {
+ sdf::ElementPtr geomElem = visualSDF->GetElement("geometry");
+ geomElem = GeometryToSDF(_msg.geometry(), geomElem);
+ }
+
+ /// Load the material
+ if (_msg.has_material())
+ {
+ sdf::ElementPtr materialElem = visualSDF->GetElement("material");
+ materialElem = MaterialToSDF(_msg.material(), materialElem);
+ }
+
+ // Set plugins of the visual
+ if (_msg.has_plugin())
+ {
+ sdf::ElementPtr pluginElem = visualSDF->GetElement("plugin");
+ pluginElem = PluginToSDF(_msg.plugin(), pluginElem);
+ }
+
+ return visualSDF;
+ }
+
+ /////////////////////////////////////////////////
+ sdf::ElementPtr MaterialToSDF(const msgs::Material &_msg,
+ sdf::ElementPtr _sdf)
+ {
+ sdf::ElementPtr materialSDF;
+
+ if (_sdf)
+ {
+ materialSDF = _sdf;
+ }
+ else
+ {
+ materialSDF.reset(new sdf::Element);
+ sdf::initFile("material.sdf", materialSDF);
+ }
+
+ if (_msg.has_script())
+ {
+ sdf::ElementPtr scriptElem = materialSDF->GetElement("script");
+ msgs::Material::Script script = _msg.script();
+
+ if (script.has_name())
+ scriptElem->GetElement("name")->Set(script.name());
+
+ if (script.uri_size() > 0)
+ while (scriptElem->HasElement("uri"))
+ scriptElem->GetElement("uri")->RemoveFromParent();
+ for (int i = 0; i < script.uri_size(); ++i)
+ {
+ sdf::ElementPtr uriElem = scriptElem->AddElement("uri");
+ uriElem->Set(script.uri(i));
+ }
+ }
+
+ if (_msg.has_shader_type())
+ {
+ sdf::ElementPtr shaderElem = materialSDF->GetElement("shader");
+ shaderElem->GetAttribute("type")->Set(
+ ConvertShaderType(_msg.shader_type()));
+ }
+
+ if (_msg.has_normal_map())
+ {
+ sdf::ElementPtr shaderElem = materialSDF->GetElement("shader");
+ shaderElem->GetElement("normal_map")->Set(_msg.normal_map());
+ }
+
+ if (_msg.has_lighting())
+ materialSDF->GetElement("lighting")->Set(_msg.lighting());
+
+ if (_msg.has_ambient())
+ materialSDF->GetElement("ambient")->Set(Convert(_msg.ambient()));
+ if (_msg.has_diffuse())
+ materialSDF->GetElement("diffuse")->Set(Convert(_msg.diffuse()));
+ if (_msg.has_emissive())
+ materialSDF->GetElement("emissive")->Set(Convert(_msg.emissive()));
+ if (_msg.has_specular())
+ materialSDF->GetElement("specular")->Set(Convert(_msg.specular()));
+
+ return materialSDF;
+ }
+
+ /////////////////////////////////////////////////
+ msgs::Material::ShaderType ConvertShaderType(const std::string &_str)
+ {
+ auto result = msgs::Material::VERTEX;
+ if (_str == "vertex")
+ {
+ result = msgs::Material::VERTEX;
+ }
+ else if (_str == "pixel")
+ {
+ result = msgs::Material::PIXEL;
+ }
+ else if (_str == "normal_map_object_space")
+ {
+ result = msgs::Material::NORMAL_MAP_OBJECT_SPACE;
+ }
+ else if (_str == "normal_map_tangent_space")
+ {
+ result = msgs::Material::NORMAL_MAP_TANGENT_SPACE;
+ }
+ else
+ {
+ gzerr << "Unrecognized ShaderType ["
+ << _str
+ << "], returning VERTEX"
+ << std::endl;
+ }
+ return result;
+ }
+
+ /////////////////////////////////////////////////
+ std::string ConvertShaderType(const msgs::Material::ShaderType _type)
+ {
+ std::string result;
+ switch (_type)
+ {
+ case msgs::Material::VERTEX:
+ {
+ result = "vertex";
+ break;
+ }
+ case msgs::Material::PIXEL:
+ {
+ result = "pixel";
+ break;
+ }
+ case msgs::Material::NORMAL_MAP_OBJECT_SPACE:
+ {
+ result = "normal_map_object_space";
+ break;
+ }
+ case msgs::Material::NORMAL_MAP_TANGENT_SPACE:
+ {
+ result = "normal_map_tangent_space";
+ break;
+ }
+ default:
+ {
+ result = "unknown";
+ gzerr << "Unrecognized ShaderType [" << _type << "]"
+ << std::endl;
+ break;
+ }
+ }
+ return result;
+ }
+
+ /////////////////////////////////////////////////
msgs::Fog FogFromSDF(sdf::ElementPtr _sdf)
{
msgs::Fog result;
@@ -1118,16 +1318,24 @@ namespace gazebo
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
inertialElem = InertialToSDF(_msg.inertial(), inertialElem);
}
- while (linkSDF->HasElement("collision"))
- linkSDF->GetElement("collision")->RemoveFromParent();
+ if (_msg.collision_size() > 0)
+ while (linkSDF->HasElement("collision"))
+ linkSDF->GetElement("collision")->RemoveFromParent();
for (int i = 0; i < _msg.collision_size(); ++i)
{
sdf::ElementPtr collisionElem = linkSDF->AddElement("collision");
collisionElem = CollisionToSDF(_msg.collision(i), collisionElem);
}
+ if (_msg.visual_size() > 0)
+ while (linkSDF->HasElement("visual"))
+ linkSDF->GetElement("visual")->RemoveFromParent();
+ for (int i = 0; i < _msg.visual_size(); ++i)
+ {
+ sdf::ElementPtr visualElem = linkSDF->AddElement("visual");
+ visualElem = VisualToSDF(_msg.visual(i), visualElem);
+ }
- gzwarn << "msgs::LinkToSDF currently does not convert visual,"
- << " sensor, and projector data";
+ /// \todo LinkToSDF currently does not convert sensor and projector data
return linkSDF;
}
@@ -1230,19 +1438,36 @@ namespace gazebo
_msg.collide_without_contact_bitmask());
}
- sdf::ElementPtr physicsEngElem = contactElem->GetElement("ode");
+ sdf::ElementPtr odeElem = contactElem->GetElement("ode");
+ sdf::ElementPtr bulletElem = contactElem->GetElement("bullet");
if (_msg.has_soft_cfm())
- physicsEngElem->GetElement("soft_cfm")->Set(_msg.soft_cfm());
+ {
+ odeElem->GetElement("soft_cfm")->Set(_msg.soft_cfm());
+ bulletElem->GetElement("soft_cfm")->Set(_msg.soft_cfm());
+ }
if (_msg.has_soft_erp())
- physicsEngElem->GetElement("soft_erp")->Set(_msg.soft_erp());
+ {
+ odeElem->GetElement("soft_erp")->Set(_msg.soft_erp());
+ bulletElem->GetElement("soft_erp")->Set(_msg.soft_erp());
+ }
if (_msg.has_kp())
- physicsEngElem->GetElement("kp")->Set(_msg.kp());
+ {
+ odeElem->GetElement("kp")->Set(_msg.kp());
+ bulletElem->GetElement("kp")->Set(_msg.kp());
+ }
if (_msg.has_kd())
- physicsEngElem->GetElement("kd")->Set(_msg.kd());
+ {
+ odeElem->GetElement("kd")->Set(_msg.kd());
+ bulletElem->GetElement("kd")->Set(_msg.kd());
+ }
if (_msg.has_max_vel())
- physicsEngElem->GetElement("max_vel")->Set(_msg.max_vel());
+ {
+ odeElem->GetElement("max_vel")->Set(_msg.max_vel());
+ }
if (_msg.has_min_depth())
- physicsEngElem->GetElement("min_depth")->Set(_msg.min_depth());
+ {
+ odeElem->GetElement("min_depth")->Set(_msg.min_depth());
+ }
return surfaceSDF;
}
@@ -1266,14 +1491,16 @@ namespace gazebo
if (!_msg.has_type())
return geometrySDF;
- if (_msg.type() == msgs::Geometry::BOX)
+ if (_msg.type() == msgs::Geometry::BOX &&
+ _msg.has_box())
{
sdf::ElementPtr geom = geometrySDF->GetElement("box");
msgs::BoxGeom boxGeom = _msg.box();
if (boxGeom.has_size())
geom->GetElement("size")->Set(msgs::Convert(boxGeom.size()));
}
- else if (_msg.type() == msgs::Geometry::CYLINDER)
+ else if (_msg.type() == msgs::Geometry::CYLINDER &&
+ _msg.has_cylinder())
{
sdf::ElementPtr geom = geometrySDF->GetElement("cylinder");
msgs::CylinderGeom cylinderGeom = _msg.cylinder();
@@ -1282,14 +1509,16 @@ namespace gazebo
if (cylinderGeom.has_length())
geom->GetElement("length")->Set(cylinderGeom.length());
}
- if (_msg.type() == msgs::Geometry::SPHERE)
+ else if (_msg.type() == msgs::Geometry::SPHERE &&
+ _msg.has_sphere())
{
sdf::ElementPtr geom = geometrySDF->GetElement("sphere");
msgs::SphereGeom sphereGeom = _msg.sphere();
if (sphereGeom.has_radius())
geom->GetElement("radius")->Set(sphereGeom.radius());
}
- if (_msg.type() == msgs::Geometry::PLANE)
+ else if (_msg.type() == msgs::Geometry::PLANE &&
+ _msg.has_plane())
{
sdf::ElementPtr geom = geometrySDF->GetElement("plane");
msgs::PlaneGeom planeGeom = _msg.plane();
@@ -1300,8 +1529,11 @@ namespace gazebo
}
if (planeGeom.has_size())
geom->GetElement("size")->Set(msgs::Convert(planeGeom.size()));
+ if (planeGeom.has_d())
+ gzerr << "sdformat doesn't have Plane.d variable" << std::endl;
}
- if (_msg.type() == msgs::Geometry::IMAGE)
+ else if (_msg.type() == msgs::Geometry::IMAGE &&
+ _msg.has_image())
{
sdf::ElementPtr geom = geometrySDF->GetElement("image");
msgs::ImageGeom imageGeom = _msg.image();
@@ -1316,7 +1548,8 @@ namespace gazebo
if (imageGeom.has_granularity())
geom->GetElement("granularity")->Set(imageGeom.granularity());
}
- if (_msg.type() == msgs::Geometry::HEIGHTMAP)
+ else if (_msg.type() == msgs::Geometry::HEIGHTMAP &&
+ _msg.has_heightmap())
{
sdf::ElementPtr geom = geometrySDF->GetElement("heightmap");
msgs::HeightmapGeom heightmapGeom = _msg.heightmap();
@@ -1335,8 +1568,9 @@ namespace gazebo
geom->GetElement("use_terrain_paging")->Set(
heightmapGeom.use_terrain_paging());
}
- while (geom->HasElement("texture"))
- geom->GetElement("texture")->RemoveFromParent();
+ if (heightmapGeom.texture_size() > 0)
+ while (geom->HasElement("texture"))
+ geom->GetElement("texture")->RemoveFromParent();
for (int i = 0; i < heightmapGeom.texture_size(); ++i)
{
gazebo::msgs::HeightmapGeom_Texture textureMsg =
@@ -1346,8 +1580,9 @@ namespace gazebo
textureElem->GetElement("normal")->Set(textureMsg.normal());
textureElem->GetElement("size")->Set(textureMsg.size());
}
- while (geom->HasElement("blend"))
- geom->GetElement("blend")->RemoveFromParent();
+ if (heightmapGeom.blend_size() > 0)
+ while (geom->HasElement("blend"))
+ geom->GetElement("blend")->RemoveFromParent();
for (int i = 0; i < heightmapGeom.blend_size(); ++i)
{
gazebo::msgs::HeightmapGeom_Blend blendMsg =
@@ -1359,20 +1594,23 @@ namespace gazebo
if (heightmapGeom.has_filename())
geom->GetElement("uri")->Set(heightmapGeom.filename());
}
- if (_msg.type() == msgs::Geometry::MESH)
+ else if (_msg.type() == msgs::Geometry::MESH &&
+ _msg.has_mesh())
{
sdf::ElementPtr geom = geometrySDF->GetElement("mesh");
msgs::MeshGeom meshGeom = _msg.mesh();
geom = msgs::MeshToSDF(meshGeom, geom);
}
- if (_msg.type() == msgs::Geometry::POLYLINE)
+ else if (_msg.type() == msgs::Geometry::POLYLINE &&
+ _msg.has_polyline())
{
sdf::ElementPtr geom = geometrySDF->GetElement("polyline");
gazebo::msgs::Polyline polylineGeom = _msg.polyline();
if (polylineGeom.has_height())
geom->GetElement("height")->Set(polylineGeom.height());
- while (geom->HasElement("point"))
- geom->GetElement("point")->RemoveFromParent();
+ if (polylineGeom.point_size() > 0)
+ while (geom->HasElement("point"))
+ geom->GetElement("point")->RemoveFromParent();
for (int i = 0; i < polylineGeom.point_size(); ++i)
{
@@ -1380,6 +1618,10 @@ namespace gazebo
pointElem->Set(msgs::Convert(polylineGeom.point(i)));
}
}
+ else
+ {
+ gzerr << "Unrecognized geometry type" << std::endl;
+ }
return geometrySDF;
}
@@ -1440,5 +1682,267 @@ namespace gazebo
return pluginSDF;
}
+
+ ////////////////////////////////////////////////////////
+ void AddLinkGeom(Model &_model, const Geometry &_geom)
+ {
+ _model.add_link();
+ int linkCount = _model.link_size();
+ auto link = _model.mutable_link(linkCount-1);
+ {
+ std::ostringstream linkName;
+ linkName << "link_" << linkCount;
+ link->set_name(linkName.str());
+ }
+
+ {
+ link->add_collision();
+ auto collision = link->mutable_collision(0);
+ collision->set_name("collision");
+ *(collision->mutable_geometry()) = _geom;
+ }
+
+ {
+ link->add_visual();
+ auto visual = link->mutable_visual(0);
+ visual->set_name("visual");
+ *(visual->mutable_geometry()) = _geom;
+
+ auto script = visual->mutable_material()->mutable_script();
+ script->add_uri();
+ script->set_uri(0, "file://media/materials/scripts/gazebo.material");
+ script->set_name("Gazebo/Grey");
+ }
+ }
+
+ ////////////////////////////////////////////////////////
+ void AddBoxLink(Model &_model, const double _mass,
+ const math::Vector3 &_size)
+ {
+ Geometry geometry;
+ geometry.set_type(Geometry_Type_BOX);
+ Set(geometry.mutable_box()->mutable_size(), _size);
+
+ AddLinkGeom(_model, geometry);
+ int linkCount = _model.link_size();
+ auto link = _model.mutable_link(linkCount-1);
+
+ auto inertial = link->mutable_inertial();
+ inertial->set_mass(_mass);
+ {
+ double dx = _size.x;
+ double dy = _size.y;
+ double dz = _size.z;
+ double ixx = _mass/12.0 * (dy*dy + dz*dz);
+ double iyy = _mass/12.0 * (dz*dz + dx*dx);
+ double izz = _mass/12.0 * (dx*dx + dy*dy);
+ inertial->set_ixx(ixx);
+ inertial->set_iyy(iyy);
+ inertial->set_izz(izz);
+ inertial->set_ixy(0.0);
+ inertial->set_ixz(0.0);
+ inertial->set_iyz(0.0);
+ }
+ }
+
+ ////////////////////////////////////////////////////////
+ void AddCylinderLink(Model &_model,
+ const double _mass,
+ const double _radius,
+ const double _length)
+ {
+ Geometry geometry;
+ geometry.set_type(Geometry_Type_CYLINDER);
+ geometry.mutable_cylinder()->set_radius(_radius);
+ geometry.mutable_cylinder()->set_length(_length);
+
+ AddLinkGeom(_model, geometry);
+ int linkCount = _model.link_size();
+ auto link = _model.mutable_link(linkCount-1);
+
+ auto inertial = link->mutable_inertial();
+ inertial->set_mass(_mass);
+ const double r2 = _radius * _radius;
+ const double ixx = _mass * (0.25 * r2 + _length*_length / 12.0);
+ const double izz = _mass * 0.5 * r2;
+ inertial->set_ixx(ixx);
+ inertial->set_iyy(ixx);
+ inertial->set_izz(izz);
+ inertial->set_ixy(0.0);
+ inertial->set_ixz(0.0);
+ inertial->set_iyz(0.0);
+ }
+
+ ////////////////////////////////////////////////////////
+ void AddSphereLink(Model &_model, const double _mass,
+ const double _radius)
+ {
+ Geometry geometry;
+ geometry.set_type(Geometry_Type_SPHERE);
+ geometry.mutable_sphere()->set_radius(_radius);
+
+ AddLinkGeom(_model, geometry);
+ int linkCount = _model.link_size();
+ auto link = _model.mutable_link(linkCount-1);
+
+ auto inertial = link->mutable_inertial();
+ inertial->set_mass(_mass);
+ const double ixx = _mass * 0.4 * _radius * _radius;
+ inertial->set_ixx(ixx);
+ inertial->set_iyy(ixx);
+ inertial->set_izz(ixx);
+ inertial->set_ixy(0.0);
+ inertial->set_ixz(0.0);
+ inertial->set_iyz(0.0);
+ }
+
+ ////////////////////////////////////////////////////////
+ sdf::ElementPtr ModelToSDF(const msgs::Model &_msg, sdf::ElementPtr _sdf)
+ {
+ sdf::ElementPtr modelSDF;
+
+ if (_sdf)
+ {
+ modelSDF = _sdf;
+ }
+ else
+ {
+ modelSDF.reset(new sdf::Element);
+ sdf::initFile("model.sdf", modelSDF);
+ }
+
+ if (_msg.has_name())
+ modelSDF->GetAttribute("name")->Set(_msg.name());
+ // ignore the id field, since it's not used in sdformat
+ if (_msg.has_is_static())
+ modelSDF->GetElement("static")->Set(_msg.is_static());
+ if (_msg.has_pose())
+ modelSDF->GetElement("pose")->Set(msgs::Convert(_msg.pose()));
+
+ if (_msg.joint_size() > 0)
+ while (modelSDF->HasElement("joint"))
+ modelSDF->GetElement("joint")->RemoveFromParent();
+ for (int i = 0; i < _msg.joint_size(); ++i)
+ {
+ sdf::ElementPtr jointElem = modelSDF->AddElement("joint");
+ jointElem = JointToSDF(_msg.joint(i), jointElem);
+ }
+
+ if (_msg.link_size())
+ while (modelSDF->HasElement("link"))
+ modelSDF->GetElement("link")->RemoveFromParent();
+ for (int i = 0; i < _msg.link_size(); ++i)
+ {
+ sdf::ElementPtr linkElem = modelSDF->AddElement("link");
+ linkElem = LinkToSDF(_msg.link(i), linkElem);
+ }
+
+ // ignore the deleted field, since it's not used in sdformat
+ if (_msg.visual_size() > 0)
+ {
+ // model element in SDF cannot store visuals,
+ // so ignore them for now
+ gzerr << "Model visuals not yet parsed" << std::endl;
+ }
+ // ignore the scale field, since it's not used in sdformat
+
+ return modelSDF;
+ }
+
+ ////////////////////////////////////////////////////////
+ sdf::ElementPtr JointToSDF(const msgs::Joint &_msg, sdf::ElementPtr _sdf)
+ {
+ sdf::ElementPtr jointSDF;
+
+ if (_sdf)
+ {
+ jointSDF = _sdf;
+ }
+ else
+ {
+ jointSDF.reset(new sdf::Element);
+ sdf::initFile("joint.sdf", jointSDF);
+ }
+
+ if (_msg.has_name())
+ jointSDF->GetAttribute("name")->Set(_msg.name());
+ if (_msg.has_type())
+ jointSDF->GetAttribute("type")->Set(ConvertJointType(_msg.type()));
+ // ignore the id field, since it's not used in sdformat
+ // ignore the parent_id field, since it's not used in sdformat
+ // ignore the child_id field, since it's not used in sdformat
+ // ignore the angle field, since it's not used in sdformat
+ if (_msg.has_parent())
+ jointSDF->GetElement("parent")->Set(_msg.parent());
+ if (_msg.has_child())
+ jointSDF->GetElement("child")->Set(_msg.child());
+ if (_msg.has_pose())
+ jointSDF->GetElement("pose")->Set(Convert(_msg.pose()));
+ if (_msg.has_axis1())
+ AxisToSDF(_msg.axis1(), jointSDF->GetElement("axis"));
+ if (_msg.has_axis2())
+ AxisToSDF(_msg.axis2(), jointSDF->GetElement("axis2"));
+
+ auto odePhysicsElem = jointSDF->GetElement("physics")->GetElement("ode");
+ if (_msg.has_cfm())
+ odePhysicsElem->GetElement("cfm")->Set(_msg.cfm());
+ if (_msg.has_bounce())
+ odePhysicsElem->GetElement("bounce")->Set(_msg.bounce());
+ if (_msg.has_velocity())
+ odePhysicsElem->GetElement("velocity")->Set(_msg.velocity());
+ if (_msg.has_fudge_factor())
+ odePhysicsElem->GetElement("fudge_factor")->Set(_msg.fudge_factor());
+
+ {
+ auto limitElem = odePhysicsElem->GetElement("limit");
+ if (_msg.has_limit_cfm())
+ limitElem->GetElement("cfm")->Set(_msg.limit_cfm());
+ if (_msg.has_limit_erp())
+ limitElem->GetElement("erp")->Set(_msg.limit_erp());
+ }
+
+ {
+ auto suspensionElem = odePhysicsElem->GetElement("suspension");
+ if (_msg.has_suspension_cfm())
+ suspensionElem->GetElement("cfm")->Set(_msg.suspension_cfm());
+ if (_msg.has_suspension_erp())
+ suspensionElem->GetElement("erp")->Set(_msg.suspension_erp());
+ }
+ /// \todo JointToSDF currently does not convert sensor data
+
+ return jointSDF;
+ }
+
+ ////////////////////////////////////////////////////////
+ void AxisToSDF(const msgs::Axis &_msg, sdf::ElementPtr _sdf)
+ {
+ if (_msg.has_xyz())
+ _sdf->GetElement("xyz")->Set(Convert(_msg.xyz()));
+ if (_msg.has_use_parent_model_frame())
+ {
+ _sdf->GetElement("use_parent_model_frame")->Set(
+ _msg.use_parent_model_frame());
+ }
+
+ {
+ auto dynamicsElem = _sdf->GetElement("dynamics");
+ if (_msg.has_damping())
+ dynamicsElem->GetElement("damping")->Set(_msg.damping());
+ if (_msg.has_friction())
+ dynamicsElem->GetElement("friction")->Set(_msg.friction());
+ }
+
+ {
+ auto limitElem = _sdf->GetElement("limit");
+ if (_msg.has_limit_lower())
+ limitElem->GetElement("lower")->Set(_msg.limit_lower());
+ if (_msg.has_limit_upper())
+ limitElem->GetElement("upper")->Set(_msg.limit_upper());
+ if (_msg.has_limit_effort())
+ limitElem->GetElement("effort")->Set(_msg.limit_effort());
+ if (_msg.has_limit_velocity())
+ limitElem->GetElement("velocity")->Set(_msg.limit_velocity());
+ }
+ }
}
}
diff --git a/gazebo/msgs/msgs.hh b/gazebo/msgs/msgs.hh
index def88e4..eb493a1 100644
--- a/gazebo/msgs/msgs.hh
+++ b/gazebo/msgs/msgs.hh
@@ -118,13 +118,15 @@ namespace gazebo
/// \brief Convert a string to a msgs::Joint::Type enum.
/// \param[in] _str Joint type string.
- /// \return A msgs::Joint::Type enum.
+ /// \return A msgs::Joint::Type enum. Defaults to REVOLUTE
+ /// if _str is unrecognized.
GAZEBO_VISIBLE
msgs::Joint::Type ConvertJointType(const std::string &_str);
/// \brief Convert a msgs::Joint::Type to a string.
/// \param[in] _type A msgs::Joint::Type enum.
- /// \return Joint type string.
+ /// \return Joint type string. Returns "unknown" if
+ /// _type is unrecognized.
GAZEBO_VISIBLE
std::string ConvertJointType(const msgs::Joint::Type _type);
@@ -268,6 +270,40 @@ namespace gazebo
GAZEBO_VISIBLE
msgs::Visual VisualFromSDF(sdf::ElementPtr _sdf);
+ /// \brief Create or update an SDF element from a msgs::Visual
+ /// \param[in] _msg Visual messsage
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
+ /// creating a new sdf element.
+ /// \return The new SDF element.
+ GAZEBO_VISIBLE
+ sdf::ElementPtr VisualToSDF(const msgs::Visual &_msg,
+ sdf::ElementPtr _sdf = sdf::ElementPtr());
+
+ /// \brief Create or update an SDF element from a msgs::Material
+ /// If _sdf is supplied and _msg has script uri's
+ /// the <uri> elements will be removed from _sdf.
+ /// \param[in] _msg Material messsage
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
+ /// creating a new sdf element.
+ /// \return The new SDF element.
+ GAZEBO_VISIBLE
+ sdf::ElementPtr MaterialToSDF(const msgs::Material &_msg,
+ sdf::ElementPtr _sdf = sdf::ElementPtr());
+
+ /// \brief Convert a string to a msgs::Material::ShaderType enum.
+ /// \param[in] _str Shader type string.
+ /// \return A msgs::Material::ShaderType enum. Defaults to VERTEX
+ /// if _str is unrecognized.
+ GAZEBO_VISIBLE
+ msgs::Material::ShaderType ConvertShaderType(const std::string &_str);
+
+ /// \brief Convert a msgs::ShaderType to a string.
+ /// \param[in] _type A msgs::ShaderType enum.
+ /// \return Shader type string. Returns "unknown" if
+ /// _type is unrecognized.
+ GAZEBO_VISIBLE
+ std::string ConvertShaderType(const msgs::Material::ShaderType _type);
+
/// \brief Create a msgs::Fog from a fog SDF element
/// \param[in] _sdf The sdf element
/// \return The new msgs::Fog object
@@ -280,36 +316,36 @@ namespace gazebo
GAZEBO_VISIBLE
msgs::Scene SceneFromSDF(sdf::ElementPtr _sdf);
- /// \brief Create an SDF element from a msgs::Light
+ /// \brief Create or update an SDF element from a msgs::Light
/// \param[in] _msg Light messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr LightToSDF(const msgs::Light &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::CameraSensor
+ /// \brief Create or update an SDF element from a msgs::CameraSensor
/// \param[in] _msg CameraSensor messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr CameraSensorToSDF(const msgs::CameraSensor &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Plugin
+ /// \brief Create or update an SDF element from a msgs::Plugin
/// \param[in] _msg Plugin messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr PluginToSDF(const msgs::Plugin &_plugin,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Collision
+ /// \brief Create or update an SDF element from a msgs::Collision
/// \param[in] _msg Collision messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
@@ -317,51 +353,119 @@ namespace gazebo
sdf::ElementPtr _sdf = sdf::ElementPtr());
/// \internal
- /// \brief Create an SDF element from a msgs::Link.
+ /// \brief Create or update an SDF element from a msgs::Link.
+ /// If _sdf is supplied and _msg has any collisions or visuals,
+ /// the <collision> and <visual> elements will be removed from _sdf.
/// \param[in] _msg Link messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr LinkToSDF(const msgs::Link &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Inertial
+ /// \brief Create or update an SDF element from a msgs::Inertial
/// \param[in] _msg Inertial messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr InertialToSDF(const msgs::Inertial &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Surface
+ /// \brief Create or update an SDF element from a msgs::Surface
/// \param[in] _msg Surface messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr SurfaceToSDF(const msgs::Surface &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Geometry
+ /// \brief Create or update an SDF element from a msgs::Geometry
+ /// If _sdf is supplied and the _msg has non-empty repeated elements,
+ /// any existing sdf elements will be removed from _sdf prior to adding
+ /// the new elements from _msg.
/// \param[in] _msg Geometry messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr GeometryToSDF(const msgs::Geometry &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Mesh
+ /// \brief Create or update an SDF element from a msgs::Mesh
/// \param[in] _msg Mesh messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr MeshToSDF(const msgs::MeshGeom &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
+ /// \brief Add a simple box link to a Model message.
+ /// The size and mass of the box are specified, and a
+ /// single collision is added, along with an inertial
+ /// block corresponding to a box of uniform density.
+ /// \param[out] _model The msgs::Model to which the link is added.
+ /// \param[in] _mass Mass of the box.
+ /// \param[in] _size Size of the box.
+ GAZEBO_VISIBLE
+ void AddBoxLink(msgs::Model &_model, const double _mass,
+ const math::Vector3 &_size);
+
+ /// \brief Add a simple cylinder link to a Model message.
+ /// The radius, length, and mass of the cylinder are specified, and a
+ /// single collision is added, along with an inertial
+ /// block corresponding to a cylinder of uniform density
+ /// with an axis of symmetry along the Z axis.
+ /// \param[out] _model The msgs::Model to which the link is added.
+ /// \param[in] _mass Mass of the cylinder.
+ /// \param[in] _radius Radius of the cylinder.
+ /// \param[in] _length Length of the cylinder.
+ GAZEBO_VISIBLE
+ void AddCylinderLink(msgs::Model &_model, const double _mass,
+ const double _radius, const double _length);
+
+ /// \brief Add a simple sphere link to a Model message.
+ /// The size and mass of the sphere are specified, and a
+ /// single collision is added, along with an inertial
+ /// block corresponding to a sphere of uniform density.
+ /// \param[out] _model The msgs::Model to which the link is added.
+ /// \param[in] _mass Mass of the sphere.
+ /// \param[in] _radius Radius of the sphere.
+ GAZEBO_VISIBLE
+ void AddSphereLink(msgs::Model &_model, const double _mass,
+ const double _radius);
+
+ /// \brief Add a link with a collision and visual
+ /// of specified geometry to a model message.
+ /// It does not set any inertial values.
+ /// \param[out] _model The msgs::Model object to receive a new link.
+ /// \param[in] _geom Geometry to be added to collision and visual.
+ GAZEBO_VISIBLE
+ void AddLinkGeom(Model &_msg, const Geometry &_geom);
+
+ /// \brief Create or update an SDF element from msgs::Model.
+ /// If _sdf is supplied and _msg has any links or joints,
+ /// the <link> and <joint> elements will be removed from _sdf.
+ /// \param[in] _msg The msgs::Model object.
+ /// \param[in] _sdf if supplied, performs an update from _sdf instead of
+ /// creating a new sdf element.
+ /// \return The new SDF element.
+ GAZEBO_VISIBLE
+ sdf::ElementPtr ModelToSDF(const msgs::Model &_msg,
+ sdf::ElementPtr _sdf = sdf::ElementPtr());
+
+ /// \brief Create or update an SDF element from msgs::Joint.
+ /// \param[in] _msg The msgs::Joint object.
+ /// \param[in] _sdf if supplied, performs an update from _sdf instead of
+ /// creating a new sdf element.
+ /// \return The new SDF element.
+ GAZEBO_VISIBLE
+ sdf::ElementPtr JointToSDF(const msgs::Joint &_msg,
+ sdf::ElementPtr _sdf = sdf::ElementPtr());
+
/// \cond
GAZEBO_VISIBLE
const google::protobuf::FieldDescriptor *GetFD(
diff --git a/gazebo/msgs/msgs_TEST.cc b/gazebo/msgs/msgs_TEST.cc
index 1e2cae8..addc9d1 100644
--- a/gazebo/msgs/msgs_TEST.cc
+++ b/gazebo/msgs/msgs_TEST.cc
@@ -234,11 +234,28 @@ TEST_F(MsgsTest, ConvertMsgsPlaneToMath)
EXPECT_TRUE(math::equal(1.0, v.d));
}
+//////////////////////////////////////////////////
+void CompareMsgsShaderTypeToString(const msgs::Material::ShaderType _type)
+{
+ EXPECT_EQ(_type, msgs::ConvertShaderType(msgs::ConvertShaderType(_type)));
+}
+
+//////////////////////////////////////////////////
+TEST_F(MsgsTest, ConvertMsgsShaderTypeToString)
+{
+ CompareMsgsShaderTypeToString(msgs::Material::NORMAL_MAP_OBJECT_SPACE);
+ CompareMsgsShaderTypeToString(msgs::Material::NORMAL_MAP_TANGENT_SPACE);
+ CompareMsgsShaderTypeToString(msgs::Material::PIXEL);
+ CompareMsgsShaderTypeToString(msgs::Material::VERTEX);
+}
+
+//////////////////////////////////////////////////
void CompareMsgsJointTypeToString(const msgs::Joint::Type _type)
{
EXPECT_EQ(_type, msgs::ConvertJointType(msgs::ConvertJointType(_type)));
}
+//////////////////////////////////////////////////
TEST_F(MsgsTest, ConvertMsgsJointTypeToString)
{
CompareMsgsJointTypeToString(msgs::Joint::REVOLUTE);
@@ -705,7 +722,10 @@ TEST_F(MsgsTest, VisualFromSDF_BadGeometryType)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- EXPECT_FALSE(sdf::readString(
+ // As of sdformat pull request 148 (released in version 2.3.1),
+ // unknown elements are now ignored with a warning message
+ // rather than causing an error.
+ EXPECT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<pose>1 1 1 1 2 3</pose>\
@@ -866,41 +886,61 @@ TEST_F(MsgsTest, MeshFromSDF)
/////////////////////////////////////////////////
TEST_F(MsgsTest, LinkToSDF)
{
+ const std::string name("test_link");
+ const math::Pose pose(math::Vector3(3, 2, 1),
+ math::Quaternion(0.5, -0.5, -0.5, 0.5));
+
msgs::Link linkMsg;
- linkMsg.set_name("test_link");
+ linkMsg.set_name(name);
linkMsg.set_self_collide(false);
linkMsg.set_gravity(true);
linkMsg.set_kinematic(false);
- msgs::Set(linkMsg.mutable_pose(), math::Pose(math::Vector3(3, 2, 1),
- math::Quaternion(0.5, -0.5, -0.5, 0.5)));
+ msgs::Set(linkMsg.mutable_pose(), pose);
+
+ const double laserRetro1 = 0.4;
+ const double laserRetro2 = 0.5;
// collision - see CollisionToSDF for a more detailed test
- msgs::Collision *collisionMsg1 = linkMsg.add_collision();
- collisionMsg1->set_laser_retro(0.4);
+ auto collisionMsg1 = linkMsg.add_collision();
+ collisionMsg1->set_laser_retro(laserRetro1);
collisionMsg1->set_max_contacts(100);
- msgs::Collision *collisionMsg2 = linkMsg.add_collision();
- collisionMsg2->set_laser_retro(0.5);
+ auto collisionMsg2 = linkMsg.add_collision();
+ collisionMsg2->set_laser_retro(laserRetro2);
collisionMsg2->set_max_contacts(300);
+ // visual - see VisualToSDF for a more detailed test
+ auto visualMsg1 = linkMsg.add_visual();
+ visualMsg1->set_laser_retro(laserRetro1);
+
+ auto visualMsg2 = linkMsg.add_visual();
+ visualMsg2->set_laser_retro(laserRetro2);
+
// inertial - see InertialToSDF for a more detailed test
- msgs::Inertial *inertialMsg = linkMsg.mutable_inertial();
+ auto inertialMsg = linkMsg.mutable_inertial();
inertialMsg->set_mass(3.5);
sdf::ElementPtr linkSDF = msgs::LinkToSDF(linkMsg);
- EXPECT_STREQ(linkSDF->Get<std::string>("name").c_str(), "test_link");
+ EXPECT_EQ(linkSDF->Get<std::string>("name"), name);
EXPECT_FALSE(linkSDF->Get<bool>("self_collide"));
EXPECT_TRUE(linkSDF->Get<bool>("gravity"));
EXPECT_FALSE(linkSDF->Get<bool>("kinematic"));
+ EXPECT_EQ(pose, linkSDF->Get<math::Pose>("pose"));
sdf::ElementPtr collisionElem1 = linkSDF->GetElement("collision");
- EXPECT_DOUBLE_EQ(collisionElem1->Get<double>("laser_retro"), 0.4);
+ EXPECT_DOUBLE_EQ(collisionElem1->Get<double>("laser_retro"), laserRetro1);
EXPECT_DOUBLE_EQ(collisionElem1->Get<double>("max_contacts"), 100);
sdf::ElementPtr collisionElem2 = collisionElem1->GetNextElement("collision");
- EXPECT_DOUBLE_EQ(collisionElem2->Get<double>("laser_retro"), 0.5);
+ EXPECT_DOUBLE_EQ(collisionElem2->Get<double>("laser_retro"), laserRetro2);
EXPECT_DOUBLE_EQ(collisionElem2->Get<double>("max_contacts"), 300);
+ sdf::ElementPtr visualElem1 = linkSDF->GetElement("visual");
+ EXPECT_DOUBLE_EQ(visualElem1->Get<double>("laser_retro"), laserRetro1);
+
+ sdf::ElementPtr visualElem2 = visualElem1->GetNextElement("visual");
+ EXPECT_DOUBLE_EQ(visualElem2->Get<double>("laser_retro"), laserRetro2);
+
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
EXPECT_DOUBLE_EQ(inertialElem->Get<double>("mass"), 3.5);
}
@@ -908,7 +948,10 @@ TEST_F(MsgsTest, LinkToSDF)
/////////////////////////////////////////////////
TEST_F(MsgsTest, CollisionToSDF)
{
+ const std::string name("collision");
+
msgs::Collision collisionMsg;
+ collisionMsg.set_name(name);
collisionMsg.set_laser_retro(0.2);
collisionMsg.set_max_contacts(5);
msgs::Set(collisionMsg.mutable_pose(), math::Pose(math::Vector3(1, 2, 3),
@@ -927,6 +970,10 @@ TEST_F(MsgsTest, CollisionToSDF)
surfaceMsg->set_bounce_threshold(1300);
sdf::ElementPtr collisionSDF = msgs::CollisionToSDF(collisionMsg);
+
+ EXPECT_TRUE(collisionSDF->HasAttribute("name"));
+ EXPECT_EQ(name, collisionSDF->Get<std::string>("name"));
+
EXPECT_DOUBLE_EQ(collisionSDF->Get<double>("laser_retro"), 0.2);
EXPECT_DOUBLE_EQ(collisionSDF->Get<double>("max_contacts"), 5);
@@ -945,6 +992,58 @@ TEST_F(MsgsTest, CollisionToSDF)
}
/////////////////////////////////////////////////
+TEST_F(MsgsTest, VisualToSDF)
+{
+ const std::string name("visual");
+ const double laserRetro = 0.2;
+ const math::Pose pose(math::Vector3(1, 2, 3), math::Quaternion(0, 0, 1, 0));
+ const double radius = 3.3;
+ const std::string materialName("Gazebo/Grey");
+ const std::string uri("pretend_this_is_a_URI");
+
+ msgs::Visual visualMsg;
+ visualMsg.set_name(name);
+ visualMsg.set_laser_retro(laserRetro);
+ msgs::Set(visualMsg.mutable_pose(), pose);
+
+ // geometry - see GeometryToSDF for a more detailed test
+ auto geomMsg = visualMsg.mutable_geometry();
+ geomMsg->set_type(msgs::Geometry::SPHERE);
+ geomMsg->mutable_sphere()->set_radius(radius);
+
+ // material - see MaterialToSDF for a more detailed test
+ auto scriptMsg = visualMsg.mutable_material()->mutable_script();
+ scriptMsg->set_name(materialName);
+ scriptMsg->add_uri();
+ scriptMsg->set_uri(0, uri);
+
+ sdf::ElementPtr visualSDF = msgs::VisualToSDF(visualMsg);
+
+ EXPECT_TRUE(visualSDF->HasAttribute("name"));
+ EXPECT_EQ(name, visualSDF->Get<std::string>("name"));
+
+ EXPECT_DOUBLE_EQ(visualSDF->Get<double>("laser_retro"), laserRetro);
+
+ EXPECT_EQ(pose, visualSDF->Get<math::Pose>("pose"));
+
+ ASSERT_TRUE(visualSDF->HasElement("geometry"));
+ sdf::ElementPtr geomElem = visualSDF->GetElement("geometry");
+ EXPECT_TRUE(geomElem->HasElement("sphere"));
+ sdf::ElementPtr sphereElem = geomElem->GetElement("sphere");
+ EXPECT_TRUE(sphereElem->HasElement("radius"));
+ EXPECT_DOUBLE_EQ(sphereElem->Get<double>("radius"), radius);
+
+ ASSERT_TRUE(visualSDF->HasElement("material"));
+ sdf::ElementPtr materialElem = visualSDF->GetElement("material");
+ EXPECT_TRUE(materialElem->HasElement("script"));
+ sdf::ElementPtr scriptElem = materialElem->GetElement("script");
+ EXPECT_TRUE(scriptElem->HasElement("name"));
+ EXPECT_EQ(materialName, scriptElem->Get<std::string>("name"));
+ EXPECT_TRUE(scriptElem->HasElement("uri"));
+ EXPECT_EQ(uri, scriptElem->Get<std::string>("uri"));
+}
+
+/////////////////////////////////////////////////
TEST_F(MsgsTest, GeometryToSDF)
{
// box
@@ -1128,33 +1227,113 @@ TEST_F(MsgsTest, MeshToSDF)
/////////////////////////////////////////////////
TEST_F(MsgsTest, InertialToSDF)
{
+ const double mass = 3.4;
+ const math::Pose pose = math::Pose(math::Vector3(1.2, 3.4, 5.6),
+ math::Quaternion(0.7071, 0.0, 0.7071, 0.0));
+ const double ixx = 0.0133;
+ const double ixy = -0.0003;
+ const double ixz = -0.0004;
+ const double iyy = 0.0116;
+ const double iyz = 0.0008;
+ const double izz = 0.0038;
+
msgs::Inertial msg;
- msg.set_mass(3.4);
- msgs::Set(msg.mutable_pose(), math::Pose(math::Vector3(1.2, 3.4, 5.6),
- math::Quaternion(0.7071, 0.0, 0.7071, 0.0)));
- msg.set_ixx(0.0133);
- msg.set_ixy(-0.0003);
- msg.set_ixz(-0.0004);
- msg.set_iyy(0.0116);
- msg.set_iyz(0.0008);
- msg.set_izz(0.0038);
+ msg.set_mass(mass);
+ msgs::Set(msg.mutable_pose(), pose);
+ msg.set_ixx(ixx);
+ msg.set_ixy(ixy);
+ msg.set_ixz(ixz);
+ msg.set_iyy(iyy);
+ msg.set_iyz(iyz);
+ msg.set_izz(izz);
sdf::ElementPtr inertialSDF = msgs::InertialToSDF(msg);
- EXPECT_DOUBLE_EQ(inertialSDF->Get<double>("mass"), 3.4);
+ EXPECT_TRUE(inertialSDF->HasElement("mass"));
+ EXPECT_DOUBLE_EQ(inertialSDF->Get<double>("mass"), mass);
+
+ EXPECT_TRUE(inertialSDF->HasElement("pose"));
+ EXPECT_EQ(inertialSDF->Get<math::Pose>("pose"), pose);
+
+ {
+ ASSERT_TRUE(inertialSDF->HasElement("inertia"));
+ sdf::ElementPtr inertiaElem = inertialSDF->GetElement("inertia");
+
+ EXPECT_TRUE(inertiaElem->HasElement("ixx"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixx"), ixx);
- EXPECT_TRUE(inertialSDF->Get<math::Pose>("pose") ==
- math::Pose(math::Vector3(1.2, 3.4, 5.6),
- math::Quaternion(0.7071, 0.0, 0.7071, 0.0)));
+ EXPECT_TRUE(inertiaElem->HasElement("ixy"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixy"), ixy);
- // inertia
- sdf::ElementPtr inertiaElem = inertialSDF->GetElement("inertia");
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixx"), 0.0133);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixy"), -0.0003);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixz"), -0.0004);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyy"), 0.0116);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyz"), 0.0008);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("izz"), 0.0038);
+ EXPECT_TRUE(inertiaElem->HasElement("ixz"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixz"), ixz);
+
+ EXPECT_TRUE(inertiaElem->HasElement("iyy"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyy"), iyy);
+
+ EXPECT_TRUE(inertiaElem->HasElement("iyz"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyz"), iyz);
+
+ EXPECT_TRUE(inertiaElem->HasElement("izz"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("izz"), izz);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, MaterialToSDF)
+{
+ msgs::Material msg;
+
+ const std::string name("Gazebo/Grey");
+ const std::string uri("file://media/materials/scripts/gazebo.material");
+ const msgs::Material::ShaderType type = msgs::Material::VERTEX;
+ const std::string normalMap("normalMap");
+ const bool lighting = true;
+ const common::Color ambient(.1, .2, .3, 1.0);
+ const common::Color diffuse(.4, .5, .6, 1.0);
+ const common::Color emissive(.5, .5, .5, 0.5);
+ const common::Color specular(.7, .8, .9, 1.0);
+
+ msg.mutable_script()->set_name(name);
+ msg.mutable_script()->add_uri();
+ msg.mutable_script()->set_uri(0, uri);
+ msg.set_shader_type(type);
+ msg.set_normal_map(normalMap);
+ msg.set_lighting(lighting);
+ msgs::Set(msg.mutable_ambient(), ambient);
+ msgs::Set(msg.mutable_diffuse(), diffuse);
+ msgs::Set(msg.mutable_emissive(), emissive);
+ msgs::Set(msg.mutable_specular(), specular);
+
+ sdf::ElementPtr materialSDF = msgs::MaterialToSDF(msg);
+
+ {
+ ASSERT_TRUE(materialSDF->HasElement("script"));
+ sdf::ElementPtr scriptElem = materialSDF->GetElement("script");
+ EXPECT_TRUE(scriptElem->HasElement("name"));
+ EXPECT_EQ(name, scriptElem->Get<std::string>("name"));
+ EXPECT_TRUE(scriptElem->HasElement("uri"));
+ EXPECT_EQ(uri, scriptElem->Get<std::string>("uri"));
+ }
+
+ {
+ ASSERT_TRUE(materialSDF->HasElement("shader"));
+ sdf::ElementPtr shaderElem = materialSDF->GetElement("shader");
+ EXPECT_TRUE(shaderElem->HasAttribute("type"));
+ EXPECT_EQ(msgs::ConvertShaderType(type),
+ shaderElem->Get<std::string>("type"));
+ EXPECT_TRUE(shaderElem->HasElement("normal_map"));
+ EXPECT_EQ(normalMap, shaderElem->Get<std::string>("normal_map"));
+ }
+
+ EXPECT_TRUE(materialSDF->HasElement("ambient"));
+ EXPECT_EQ(ambient, materialSDF->Get<common::Color>("ambient"));
+ EXPECT_TRUE(materialSDF->HasElement("diffuse"));
+ EXPECT_EQ(diffuse, materialSDF->Get<common::Color>("diffuse"));
+ EXPECT_TRUE(materialSDF->HasElement("emissive"));
+ EXPECT_EQ(emissive, materialSDF->Get<common::Color>("emissive"));
+ EXPECT_TRUE(materialSDF->HasElement("specular"));
+ EXPECT_EQ(specular, materialSDF->Get<common::Color>("specular"));
}
/////////////////////////////////////////////////
@@ -1163,12 +1342,18 @@ TEST_F(MsgsTest, SurfaceToSDF)
msgs::Surface msg;
// friction
+ const double mu = 0.1;
+ const double mu2 = 0.2;
+ const math::Vector3 fdir1(0.3, 0.4, 0.5);
+ const double slip1 = 0.6;
+ const double slip2 = 0.7;
+
msgs::Friction *friction = msg.mutable_friction();
- friction->set_mu(0.1);
- friction->set_mu2(0.2);
- msgs::Set(friction->mutable_fdir1(), math::Vector3(0.3, 0.4, 0.5));
- friction->set_slip1(0.6);
- friction->set_slip2(0.7);
+ friction->set_mu(mu);
+ friction->set_mu2(mu2);
+ msgs::Set(friction->mutable_fdir1(), fdir1);
+ friction->set_slip1(slip1);
+ friction->set_slip2(slip2);
// bounce
msg.set_restitution_coefficient(1.1);
@@ -1187,12 +1372,11 @@ TEST_F(MsgsTest, SurfaceToSDF)
sdf::ElementPtr surfaceSDF = msgs::SurfaceToSDF(msg);
sdf::ElementPtr frictionElem = surfaceSDF->GetElement("friction");
sdf::ElementPtr frictionPhysicsElem = frictionElem->GetElement("ode");
- EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu"), 0.1);
- EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu2"), 0.2);
- EXPECT_TRUE(frictionPhysicsElem->Get<math::Vector3>("fdir1") ==
- math::Vector3(0.3, 0.4, 0.5));
- EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip1"), 0.6);
- EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip2"), 0.7);
+ EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu"), mu);
+ EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu2"), mu2);
+ EXPECT_TRUE(frictionPhysicsElem->Get<math::Vector3>("fdir1") == fdir1);
+ EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip1"), slip1);
+ EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip2"), slip2);
sdf::ElementPtr bounceElem = surfaceSDF->GetElement("bounce");
EXPECT_DOUBLE_EQ(bounceElem->Get<double>("restitution_coefficient"), 1.1);
@@ -1211,3 +1395,480 @@ TEST_F(MsgsTest, SurfaceToSDF)
EXPECT_EQ(contactElem->Get<unsigned int>("collide_without_contact_bitmask"),
static_cast<unsigned int>(0x0004));
}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, JointToSDF)
+{
+ const std::string name("test_joint");
+ const msgs::Joint::Type type = msgs::Joint::UNIVERSAL;
+ const math::Pose pose(math::Vector3(9, 1, 1), math::Quaternion(0, 1, 0, 0));
+ const std::string parent("parent_link");
+ const std::string child("child_link");
+
+ const double cfm = 0.1;
+ const double bounce = 0.2;
+ const double velocity = 0.6;
+ const double fudge_factor = 0.7;
+ const double limit_cfm = 0.3;
+ const double limit_erp = 0.4;
+ const double suspension_cfm = 0.8;
+ const double suspension_erp = 0.9;
+ const math::Vector3 xyz1(0.6, 0.8, 0.0);
+ const math::Vector3 xyz2(0.0, 0.0, 1.0);
+ const double limit_lower1 = -2.0;
+ const double limit_lower2 = -4.0;
+ const double limit_upper1 = 12.0;
+ const double limit_upper2 = 24.0;
+ const double limit_effort1 = 1e3;
+ const double limit_effort2 = 1e4;
+ const double limit_velocity1 = 33;
+ const double limit_velocity2 = 44;
+ const double damping1 = 1e-2;
+ const double damping2 = 3e-2;
+ const double friction1 = 1e2;
+ const double friction2 = 3e2;
+ const bool useParentModelFrame1 = true;
+ // don't set use_parent_model_frame for axis2
+ // expect it to match sdformat default (false)
+
+ msgs::Joint jointMsg;
+ jointMsg.set_name(name);
+ jointMsg.set_type(type);
+ jointMsg.set_parent(parent);
+ jointMsg.set_child(child);
+ msgs::Set(jointMsg.mutable_pose(), pose);
+ jointMsg.set_cfm(cfm);
+ jointMsg.set_bounce(bounce);
+ jointMsg.set_velocity(velocity);
+ jointMsg.set_fudge_factor(fudge_factor);
+ jointMsg.set_limit_cfm(limit_cfm);
+ jointMsg.set_limit_erp(limit_erp);
+ jointMsg.set_suspension_cfm(suspension_cfm);
+ jointMsg.set_suspension_erp(suspension_erp);
+ {
+ auto axis1 = jointMsg.mutable_axis1();
+ msgs::Set(axis1->mutable_xyz(), xyz1);
+ axis1->set_limit_lower(limit_lower1);
+ axis1->set_limit_upper(limit_upper1);
+ axis1->set_limit_effort(limit_effort1);
+ axis1->set_limit_velocity(limit_velocity1);
+ axis1->set_damping(damping1);
+ axis1->set_friction(friction1);
+ axis1->set_use_parent_model_frame(useParentModelFrame1);
+ }
+ {
+ auto axis2 = jointMsg.mutable_axis2();
+ msgs::Set(axis2->mutable_xyz(), xyz2);
+ axis2->set_limit_lower(limit_lower2);
+ axis2->set_limit_upper(limit_upper2);
+ axis2->set_limit_effort(limit_effort2);
+ axis2->set_limit_velocity(limit_velocity2);
+ axis2->set_damping(damping2);
+ axis2->set_friction(friction2);
+ }
+
+ sdf::ElementPtr jointSDF = msgs::JointToSDF(jointMsg);
+ EXPECT_TRUE(jointSDF->HasAttribute("name"));
+ EXPECT_EQ(jointSDF->Get<std::string>("name"), name);
+ EXPECT_TRUE(jointSDF->HasAttribute("type"));
+ EXPECT_STREQ(jointSDF->Get<std::string>("type").c_str(), "universal");
+ EXPECT_TRUE(jointSDF->HasElement("parent"));
+ EXPECT_EQ(jointSDF->Get<std::string>("parent"), parent);
+ EXPECT_TRUE(jointSDF->HasElement("child"));
+ EXPECT_EQ(jointSDF->Get<std::string>("child"), child);
+ EXPECT_TRUE(jointSDF->HasElement("pose"));
+ EXPECT_EQ(pose, jointSDF->Get<math::Pose>("pose"));
+
+ EXPECT_TRUE(jointSDF->HasElement("axis"));
+ {
+ auto axisElem = jointSDF->GetElement("axis");
+ EXPECT_TRUE(axisElem->HasElement("xyz"));
+ EXPECT_EQ(xyz1, axisElem->Get<math::Vector3>("xyz"));
+ EXPECT_TRUE(axisElem->HasElement("use_parent_model_frame"));
+ EXPECT_EQ(useParentModelFrame1,
+ axisElem->Get<bool>("use_parent_model_frame"));
+
+ EXPECT_TRUE(axisElem->HasElement("dynamics"));
+ auto axisDynamics = axisElem->GetElement("dynamics");
+ EXPECT_TRUE(axisDynamics->HasElement("damping"));
+ EXPECT_DOUBLE_EQ(damping1, axisDynamics->Get<double>("damping"));
+ EXPECT_TRUE(axisDynamics->HasElement("friction"));
+ EXPECT_DOUBLE_EQ(friction1, axisDynamics->Get<double>("friction"));
+
+ EXPECT_TRUE(axisElem->HasElement("limit"));
+ auto axisLimit = axisElem->GetElement("limit");
+ EXPECT_TRUE(axisLimit->HasElement("lower"));
+ EXPECT_DOUBLE_EQ(limit_lower1, axisLimit->Get<double>("lower"));
+ EXPECT_TRUE(axisLimit->HasElement("upper"));
+ EXPECT_DOUBLE_EQ(limit_upper1, axisLimit->Get<double>("upper"));
+ EXPECT_TRUE(axisLimit->HasElement("effort"));
+ EXPECT_DOUBLE_EQ(limit_effort1, axisLimit->Get<double>("effort"));
+ EXPECT_TRUE(axisLimit->HasElement("velocity"));
+ EXPECT_DOUBLE_EQ(limit_velocity1, axisLimit->Get<double>("velocity"));
+ }
+
+ EXPECT_TRUE(jointSDF->HasElement("axis2"));
+ {
+ auto axisElem = jointSDF->GetElement("axis2");
+ EXPECT_TRUE(axisElem->HasElement("xyz"));
+ EXPECT_EQ(xyz2, axisElem->Get<math::Vector3>("xyz"));
+ // use_parent_model_frame is required in axis.proto
+ // so expect to to exist even if we don't set it
+ EXPECT_TRUE(axisElem->HasElement("use_parent_model_frame"));
+ // expect false (default sdformat value)
+ EXPECT_FALSE(axisElem->Get<bool>("use_parent_model_frame"));
+
+ EXPECT_TRUE(axisElem->HasElement("dynamics"));
+ auto axisDynamics = axisElem->GetElement("dynamics");
+ EXPECT_TRUE(axisDynamics->HasElement("damping"));
+ EXPECT_DOUBLE_EQ(damping2, axisDynamics->Get<double>("damping"));
+ EXPECT_TRUE(axisDynamics->HasElement("friction"));
+ EXPECT_DOUBLE_EQ(friction2, axisDynamics->Get<double>("friction"));
+
+ EXPECT_TRUE(axisElem->HasElement("limit"));
+ auto axisLimit = axisElem->GetElement("limit");
+ EXPECT_TRUE(axisLimit->HasElement("lower"));
+ EXPECT_DOUBLE_EQ(limit_lower2, axisLimit->Get<double>("lower"));
+ EXPECT_TRUE(axisLimit->HasElement("upper"));
+ EXPECT_DOUBLE_EQ(limit_upper2, axisLimit->Get<double>("upper"));
+ EXPECT_TRUE(axisLimit->HasElement("effort"));
+ EXPECT_DOUBLE_EQ(limit_effort2, axisLimit->Get<double>("effort"));
+ EXPECT_TRUE(axisLimit->HasElement("velocity"));
+ EXPECT_DOUBLE_EQ(limit_velocity2, axisLimit->Get<double>("velocity"));
+ }
+
+ EXPECT_TRUE(jointSDF->HasElement("physics"));
+ auto physicsElem = jointSDF->GetElement("physics");
+ EXPECT_TRUE(physicsElem->HasElement("ode"));
+ auto odePhysics = physicsElem->GetElement("ode");
+ EXPECT_TRUE(odePhysics->HasElement("cfm"));
+ EXPECT_DOUBLE_EQ(odePhysics->Get<double>("cfm"), cfm);
+ EXPECT_TRUE(odePhysics->HasElement("bounce"));
+ EXPECT_DOUBLE_EQ(odePhysics->Get<double>("bounce"), bounce);
+ EXPECT_TRUE(odePhysics->HasElement("velocity"));
+ EXPECT_DOUBLE_EQ(odePhysics->Get<double>("velocity"), velocity);
+ EXPECT_TRUE(odePhysics->HasElement("fudge_factor"));
+ EXPECT_DOUBLE_EQ(odePhysics->Get<double>("fudge_factor"), fudge_factor);
+
+ EXPECT_TRUE(odePhysics->HasElement("limit"));
+ auto limitElem = odePhysics->GetElement("limit");
+ EXPECT_TRUE(limitElem->HasElement("cfm"));
+ EXPECT_DOUBLE_EQ(limitElem->Get<double>("cfm"), limit_cfm);
+ EXPECT_TRUE(limitElem->HasElement("erp"));
+ EXPECT_DOUBLE_EQ(limitElem->Get<double>("erp"), limit_erp);
+
+ EXPECT_TRUE(odePhysics->HasElement("suspension"));
+ auto suspensionElem = odePhysics->GetElement("suspension");
+ EXPECT_TRUE(suspensionElem->HasElement("cfm"));
+ EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("cfm"), suspension_cfm);
+ EXPECT_TRUE(suspensionElem->HasElement("erp"));
+ EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("erp"), suspension_erp);
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, AddBoxLink)
+{
+ msgs::Model model;
+ EXPECT_EQ(model.link_size(), 0);
+
+ const double mass = 1.0;
+ const math::Vector3 size(1, 1, 1);
+ msgs::AddBoxLink(model, mass, size);
+ EXPECT_EQ(model.link_size(), 1);
+ {
+ auto link = model.link(0);
+ EXPECT_EQ(link.name(), std::string("link_1"));
+
+ auto inertial = link.inertial();
+ EXPECT_DOUBLE_EQ(inertial.mass(), mass);
+ double ixx = inertial.ixx();
+ double iyy = inertial.iyy();
+ double izz = inertial.izz();
+ double ixy = inertial.ixy();
+ double ixz = inertial.ixz();
+ double iyz = inertial.iyz();
+ EXPECT_GT(ixx, 0.0);
+ EXPECT_GT(iyy, 0.0);
+ EXPECT_GT(izz, 0.0);
+ EXPECT_DOUBLE_EQ(ixy, 0.0);
+ EXPECT_DOUBLE_EQ(ixz, 0.0);
+ EXPECT_DOUBLE_EQ(iyz, 0.0);
+ // triangle inequality
+ EXPECT_GT(ixx + iyy, izz);
+ EXPECT_GT(iyy + izz, ixx);
+ EXPECT_GT(izz + ixx, iyy);
+
+ EXPECT_EQ(link.collision_size(), 1);
+ {
+ auto collision = link.collision(0);
+ auto geometry = collision.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_BOX);
+ EXPECT_EQ(msgs::Convert(geometry.box().size()), size);
+ }
+
+ EXPECT_EQ(link.visual_size(), 1);
+ {
+ auto visual = link.visual(0);
+ auto geometry = visual.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_BOX);
+ EXPECT_EQ(msgs::Convert(geometry.box().size()), size);
+ }
+ }
+
+ const double massRatio = 2.0;
+ msgs::AddBoxLink(model, mass*massRatio, size);
+ EXPECT_EQ(model.link_size(), 2);
+ {
+ auto link1 = model.link(0);
+ auto link2 = model.link(1);
+ EXPECT_EQ(link2.name(), std::string("link_2"));
+
+ auto inertial1 = link1.inertial();
+ auto inertial2 = link2.inertial();
+
+ EXPECT_NEAR(massRatio * inertial1.mass(), inertial2.mass(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.ixx(), inertial2.ixx(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.iyy(), inertial2.iyy(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.izz(), inertial2.izz(), 1e-6);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, AddCylinderLink)
+{
+ msgs::Model model;
+ EXPECT_EQ(model.link_size(), 0);
+
+ const double mass = 1.0;
+ const double radius = 0.5;
+ const double length = 2.5;
+ msgs::AddCylinderLink(model, mass, radius, length);
+ EXPECT_EQ(model.link_size(), 1);
+ {
+ auto link = model.link(0);
+ EXPECT_EQ(link.name(), std::string("link_1"));
+
+ auto inertial = link.inertial();
+ EXPECT_DOUBLE_EQ(inertial.mass(), mass);
+ double ixx = inertial.ixx();
+ double iyy = inertial.iyy();
+ double izz = inertial.izz();
+ double ixy = inertial.ixy();
+ double ixz = inertial.ixz();
+ double iyz = inertial.iyz();
+ EXPECT_GT(ixx, 0.0);
+ EXPECT_GT(iyy, 0.0);
+ EXPECT_GT(izz, 0.0);
+ EXPECT_DOUBLE_EQ(ixy, 0.0);
+ EXPECT_DOUBLE_EQ(ixz, 0.0);
+ EXPECT_DOUBLE_EQ(iyz, 0.0);
+ // triangle inequality
+ EXPECT_GT(ixx + iyy, izz);
+ EXPECT_GT(iyy + izz, ixx);
+ EXPECT_GT(izz + ixx, iyy);
+
+ EXPECT_EQ(link.collision_size(), 1);
+ {
+ auto collision = link.collision(0);
+ auto geometry = collision.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_CYLINDER);
+ EXPECT_DOUBLE_EQ(geometry.cylinder().radius(), radius);
+ EXPECT_DOUBLE_EQ(geometry.cylinder().length(), length);
+ }
+
+ EXPECT_EQ(link.visual_size(), 1);
+ {
+ auto visual = link.visual(0);
+ auto geometry = visual.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_CYLINDER);
+ EXPECT_DOUBLE_EQ(geometry.cylinder().radius(), radius);
+ EXPECT_DOUBLE_EQ(geometry.cylinder().length(), length);
+ }
+ }
+
+ const double massRatio = 2.0;
+ msgs::AddCylinderLink(model, mass*massRatio, radius, length);
+ EXPECT_EQ(model.link_size(), 2);
+ {
+ auto link1 = model.link(0);
+ auto link2 = model.link(1);
+ EXPECT_EQ(link2.name(), std::string("link_2"));
+
+ auto inertial1 = link1.inertial();
+ auto inertial2 = link2.inertial();
+
+ EXPECT_NEAR(massRatio * inertial1.mass(), inertial2.mass(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.ixx(), inertial2.ixx(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.iyy(), inertial2.iyy(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.izz(), inertial2.izz(), 1e-6);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, AddSphereLink)
+{
+ msgs::Model model;
+ EXPECT_EQ(model.link_size(), 0);
+
+ const double mass = 1.0;
+ const double radius = 0.5;
+ msgs::AddSphereLink(model, mass, radius);
+ EXPECT_EQ(model.link_size(), 1);
+ {
+ auto link = model.link(0);
+ EXPECT_EQ(link.name(), std::string("link_1"));
+
+ auto inertial = link.inertial();
+ EXPECT_DOUBLE_EQ(inertial.mass(), mass);
+ double ixx = inertial.ixx();
+ double iyy = inertial.iyy();
+ double izz = inertial.izz();
+ double ixy = inertial.ixy();
+ double ixz = inertial.ixz();
+ double iyz = inertial.iyz();
+ EXPECT_GT(ixx, 0.0);
+ EXPECT_GT(iyy, 0.0);
+ EXPECT_GT(izz, 0.0);
+ EXPECT_DOUBLE_EQ(ixy, 0.0);
+ EXPECT_DOUBLE_EQ(ixz, 0.0);
+ EXPECT_DOUBLE_EQ(iyz, 0.0);
+ // triangle inequality
+ EXPECT_GT(ixx + iyy, izz);
+ EXPECT_GT(iyy + izz, ixx);
+ EXPECT_GT(izz + ixx, iyy);
+
+ EXPECT_EQ(link.collision_size(), 1);
+ {
+ auto collision = link.collision(0);
+ auto geometry = collision.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_SPHERE);
+ EXPECT_DOUBLE_EQ(geometry.sphere().radius(), radius);
+ }
+
+ EXPECT_EQ(link.visual_size(), 1);
+ {
+ auto visual = link.visual(0);
+ auto geometry = visual.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_SPHERE);
+ EXPECT_DOUBLE_EQ(geometry.sphere().radius(), radius);
+ }
+ }
+
+ const double massRatio = 2.0;
+ msgs::AddSphereLink(model, mass*massRatio, radius);
+ EXPECT_EQ(model.link_size(), 2);
+ {
+ auto link1 = model.link(0);
+ auto link2 = model.link(1);
+ EXPECT_EQ(link2.name(), std::string("link_2"));
+
+ auto inertial1 = link1.inertial();
+ auto inertial2 = link2.inertial();
+
+ EXPECT_NEAR(massRatio * inertial1.mass(), inertial2.mass(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.ixx(), inertial2.ixx(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.iyy(), inertial2.iyy(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.izz(), inertial2.izz(), 1e-6);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, ModelToSDF)
+{
+ const std::string name("test_bicycle");
+ const math::Pose pose(math::Vector3(6, 1, 7),
+ math::Quaternion(0.5, 0.5, 0.5, 0.5));
+
+ msgs::Model model;
+ model.set_name(name);
+ model.set_is_static(false);
+ msgs::Set(model.mutable_pose(), pose);
+ EXPECT_EQ(model.link_size(), 0);
+ EXPECT_EQ(model.joint_size(), 0);
+
+ // This will be a bicycle with two wheels.
+ // The frame is a box.
+ const double length = 1.5;
+ const double height = 0.9;
+ const double width = 0.1;
+ const math::Vector3 boxSize(length, width, height);
+ const double boxMass = 4.0;
+ AddBoxLink(model, boxMass, boxSize);
+ ASSERT_EQ(model.link_size(), 1);
+ EXPECT_EQ(model.joint_size(), 0);
+ model.mutable_link(0)->set_name("frame");
+
+ // The rear wheel is a cylinder.
+ const double radius = height / 2;
+ AddCylinderLink(model, 0.5, radius, radius);
+ ASSERT_EQ(model.link_size(), 2);
+ EXPECT_EQ(model.joint_size(), 0);
+ const math::Pose cylinderPose(-length/2, 0, -height/2, M_PI/2, 0, 0);
+ {
+ auto link = model.mutable_link(1);
+ msgs::Set(link->mutable_pose(), cylinderPose);
+ link->set_name("rear_wheel");
+ }
+
+ // The front wheel is a sphere.
+ AddSphereLink(model, 0.5, radius);
+ ASSERT_EQ(model.link_size(), 3);
+ EXPECT_EQ(model.joint_size(), 0);
+ const math::Pose spherePose(length/2, 0, -height/2, 0, 0, 0);
+ {
+ auto link = model.mutable_link(2);
+ msgs::Set(link->mutable_pose(), spherePose);
+ link->set_name("front_wheel");
+ }
+
+ // Add revolute joints for the wheels.
+ // Front wheel joint
+ model.add_joint();
+ ASSERT_EQ(model.joint_size(), 1);
+ auto frontJoint = model.mutable_joint(0);
+ frontJoint->set_name("front_hinge");
+ frontJoint->set_type(msgs::ConvertJointType("revolute"));
+ frontJoint->set_parent("frame");
+ frontJoint->set_child("front_wheel");
+ const math::Vector3 frontAxis(0, 1, 0);
+ msgs::Set(frontJoint->mutable_axis1()->mutable_xyz(), frontAxis);
+
+ // Rear wheel joint
+ model.add_joint();
+ ASSERT_EQ(model.joint_size(), 2);
+ auto rearJoint = model.mutable_joint(1);
+ rearJoint->set_name("rear_hinge");
+ rearJoint->set_type(msgs::ConvertJointType("revolute"));
+ rearJoint->set_parent("frame");
+ rearJoint->set_child("rear_wheel");
+ const math::Vector3 rearAxis(0, 0, 1);
+ msgs::Set(rearJoint->mutable_axis1()->mutable_xyz(), rearAxis);
+
+ sdf::ElementPtr modelSDF = msgs::ModelToSDF(model);
+ EXPECT_EQ(modelSDF->Get<std::string>("name"), name);
+ EXPECT_FALSE(modelSDF->Get<bool>("static"));
+ EXPECT_EQ(pose, modelSDF->Get<math::Pose>("pose"));
+
+ sdf::ElementPtr linkElem1 = modelSDF->GetElement("link");
+ EXPECT_EQ(linkElem1->Get<std::string>("name"), "frame");
+ EXPECT_EQ(linkElem1->Get<math::Pose>("pose"), math::Pose());
+
+ sdf::ElementPtr linkElem2 = linkElem1->GetNextElement("link");
+ EXPECT_EQ(linkElem2->Get<std::string>("name"), "rear_wheel");
+ EXPECT_EQ(linkElem2->Get<math::Pose>("pose"), cylinderPose);
+
+ sdf::ElementPtr linkElem3 = linkElem2->GetNextElement("link");
+ EXPECT_EQ(linkElem3->Get<std::string>("name"), "front_wheel");
+ EXPECT_EQ(linkElem3->Get<math::Pose>("pose"), spherePose);
+
+ sdf::ElementPtr jointElem1 = modelSDF->GetElement("joint");
+ EXPECT_EQ(jointElem1->Get<std::string>("name"), "front_hinge");
+ EXPECT_EQ(jointElem1->Get<std::string>("type"), "revolute");
+ EXPECT_EQ(jointElem1->Get<math::Pose>("pose"), math::Pose());
+
+ sdf::ElementPtr jointElem2 = jointElem1->GetNextElement("joint");
+ EXPECT_EQ(jointElem2->Get<std::string>("name"), "rear_hinge");
+ EXPECT_EQ(jointElem2->Get<std::string>("type"), "revolute");
+ EXPECT_EQ(jointElem2->Get<math::Pose>("pose"), math::Pose());
+}
+
diff --git a/gazebo/physics/HeightmapShape.cc b/gazebo/physics/HeightmapShape.cc
index 8cf21a4..f750ea0 100644
--- a/gazebo/physics/HeightmapShape.cc
+++ b/gazebo/physics/HeightmapShape.cc
@@ -22,8 +22,11 @@
#include <gazebo/gazebo_config.h>
#ifdef HAVE_GDAL
-# include <gdal/gdalwarper.h>
-# include <gdal/gdal_priv.h>
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wfloat-equal"
+# include <gdalwarper.h>
+# include <gdal_priv.h>
+# pragma GCC diagnostic pop
#endif
#include <algorithm>
diff --git a/gazebo/physics/Joint.cc b/gazebo/physics/Joint.cc
index 7fe37b3..e8bcc44 100644
--- a/gazebo/physics/Joint.cc
+++ b/gazebo/physics/Joint.cc
@@ -448,10 +448,19 @@ void Joint::SetModel(ModelPtr _model)
}
//////////////////////////////////////////////////
-double Joint::GetParam(const std::string &/*_key*/,
- unsigned int /*_index*/)
+double Joint::GetParam(const std::string &_key, unsigned int _index)
{
- gzerr << "GetParam not yet implemented"
+ if (_key == "hi_stop")
+ {
+ return this->GetHighStop(_index).Radian();
+ }
+ else if (_key == "lo_stop")
+ {
+ return this->GetLowStop(_index).Radian();
+ }
+ gzerr << "GetParam unrecognized parameter ["
+ << _key
+ << "]"
<< std::endl;
return 0;
}
diff --git a/gazebo/physics/Link.cc b/gazebo/physics/Link.cc
index 7ec37a3..81a5584 100644
--- a/gazebo/physics/Link.cc
+++ b/gazebo/physics/Link.cc
@@ -619,6 +619,12 @@ math::Vector3 Link::GetWorldAngularAccel() const
}
//////////////////////////////////////////////////
+math::Vector3 Link::GetWorldAngularMomentum() const
+{
+ return this->GetWorldInertiaMatrix() * this->GetWorldAngularVel();
+}
+
+//////////////////////////////////////////////////
math::Vector3 Link::GetRelativeForce() const
{
return this->GetWorldPose().rot.RotateVectorReverse(this->GetWorldForce());
diff --git a/gazebo/physics/Link.hh b/gazebo/physics/Link.hh
index 66d30de..33f7ae4 100644
--- a/gazebo/physics/Link.hh
+++ b/gazebo/physics/Link.hh
@@ -256,6 +256,13 @@ namespace gazebo
/// \return Angular acceleration of the body.
public: math::Vector3 GetRelativeAngularAccel() const;
+ /// \brief Get the angular momentum of the body CoG in the world frame,
+ /// which is computed as (I * w), where
+ /// I: inertia matrix in world frame
+ /// w: angular velocity in world frame
+ /// \return Angular momentum of the body.
+ public: math::Vector3 GetWorldAngularMomentum() const;
+
/// \brief Get the angular acceleration of the body in the world
/// frame.
/// \return Angular acceleration of the body in the world frame.
diff --git a/gazebo/physics/PhysicsIface.cc b/gazebo/physics/PhysicsIface.cc
index 4c7783d..7fb84d3 100644
--- a/gazebo/physics/PhysicsIface.cc
+++ b/gazebo/physics/PhysicsIface.cc
@@ -64,11 +64,10 @@ physics::WorldPtr physics::get_world(const std::string &_name)
}
else
{
- for (std::vector<WorldPtr>::iterator iter = g_worlds.begin();
- iter != g_worlds.end(); ++iter)
+ for (auto const &world : g_worlds)
{
- if ((*iter)->GetName() == _name)
- return (*iter);
+ if (world->GetName() == _name)
+ return world;
}
}
@@ -80,41 +79,36 @@ physics::WorldPtr physics::get_world(const std::string &_name)
/////////////////////////////////////////////////
void physics::load_worlds(sdf::ElementPtr _sdf)
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->Load(_sdf);
+ for (auto &world : g_worlds)
+ world->Load(_sdf);
}
/////////////////////////////////////////////////
void physics::init_worlds()
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->Init();
+ for (auto &world : g_worlds)
+ world->Init();
}
/////////////////////////////////////////////////
void physics::run_worlds(unsigned int _steps)
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->Run(_steps);
+ for (auto &world : g_worlds)
+ world->Run(_steps);
}
/////////////////////////////////////////////////
void physics::pause_worlds(bool _pause)
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->SetPaused(_pause);
+ for (auto &world : g_worlds)
+ world->SetPaused(_pause);
}
/////////////////////////////////////////////////
void physics::stop_worlds()
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->Stop();
+ for (auto &world : g_worlds)
+ world->Stop();
}
/////////////////////////////////////////////////
@@ -150,11 +144,10 @@ void physics::stop_world(WorldPtr _world)
/////////////////////////////////////////////////
void physics::remove_worlds()
{
- for (std::vector<WorldPtr>::iterator iter = g_worlds.begin();
- iter != g_worlds.end(); ++iter)
+ for (auto &world : g_worlds)
{
- (*iter)->Fini();
- (*iter).reset();
+ world->Fini();
+ world.reset();
}
g_worlds.clear();
@@ -163,10 +156,9 @@ void physics::remove_worlds()
/////////////////////////////////////////////////
bool physics::worlds_running()
{
- for (std::vector<WorldPtr>::const_iterator iter = g_worlds.begin();
- iter != g_worlds.end(); ++iter)
+ for (auto const &world : g_worlds)
{
- if ((*iter)->GetRunning())
+ if (world->GetRunning())
return true;
}
diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc
index fd32e05..5710162 100644
--- a/gazebo/physics/World.cc
+++ b/gazebo/physics/World.cc
@@ -647,7 +647,7 @@ void World::Step(unsigned int _steps)
bool wait = true;
while (wait)
{
- common::Time::MSleep(1);
+ common::Time::NSleep(1);
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->worldUpdateMutex);
if (this->dataPtr->stepInc == 0 || this->dataPtr->stop)
wait = false;
@@ -718,11 +718,9 @@ void World::Update()
boost::recursive_mutex::scoped_lock lock(
*this->dataPtr->physicsEngine->GetPhysicsUpdateMutex());
- for (std::list<Entity*>::iterator iter =
- this->dataPtr->dirtyPoses.begin();
- iter != this->dataPtr->dirtyPoses.end(); ++iter)
+ for (auto &dirtyEntity : this->dataPtr->dirtyPoses)
{
- (*iter)->SetWorldPose((*iter)->GetDirtyPose(), false);
+ dirtyEntity->SetWorldPose(dirtyEntity->GetDirtyPose(), false);
}
this->dataPtr->dirtyPoses.clear();
@@ -793,10 +791,9 @@ void World::ClearModels()
this->dataPtr->publishModelPoses.clear();
// Remove all models
- for (Model_V::iterator iter = this->dataPtr->models.begin();
- iter != this->dataPtr->models.end(); ++iter)
+ for (auto &model : this->dataPtr->models)
{
- this->dataPtr->rootElement->RemoveChild((*iter)->GetId());
+ this->dataPtr->rootElement->RemoveChild(model->GetId());
}
this->dataPtr->models.clear();
@@ -1013,11 +1010,9 @@ void World::Reset()
this->ResetTime();
this->ResetEntities(Base::BASE);
- for (std::vector<WorldPluginPtr>::iterator iter =
- this->dataPtr->plugins.begin();
- iter != this->dataPtr->plugins.end(); ++iter)
+ for (auto &plugin : this->dataPtr->plugins)
{
- (*iter)->Reset();
+ plugin->Reset();
}
this->dataPtr->physicsEngine->Reset();
@@ -1296,13 +1291,12 @@ void World::LoadPlugin(const std::string &_filename,
//////////////////////////////////////////////////
void World::RemovePlugin(const std::string &_name)
{
- std::vector<WorldPluginPtr>::iterator iter;
- for (iter = this->dataPtr->plugins.begin();
- iter != this->dataPtr->plugins.end(); ++iter)
+ for (auto plugin = this->dataPtr->plugins.begin();
+ plugin != this->dataPtr->plugins.end(); ++plugin)
{
- if ((*iter)->GetHandle() == _name)
+ if ((*plugin)->GetHandle() == _name)
{
- this->dataPtr->plugins.erase(iter);
+ this->dataPtr->plugins.erase(plugin);
break;
}
}
@@ -1321,11 +1315,9 @@ void World::ProcessEntityMsgs()
{
boost::mutex::scoped_lock lock(this->dataPtr->entityDeleteMutex);
- std::list<std::string>::iterator iter;
- for (iter = this->dataPtr->deleteEntity.begin();
- iter != this->dataPtr->deleteEntity.end(); ++iter)
+ for (auto &entityName : this->dataPtr->deleteEntity)
{
- this->RemoveModel(*iter);
+ this->RemoveModel(entityName);
}
if (!this->dataPtr->deleteEntity.empty())
@@ -1341,16 +1333,14 @@ void World::ProcessRequestMsgs()
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
msgs::Response response;
- std::list<msgs::Request>::iterator iter;
- for (iter = this->dataPtr->requestMsgs.begin();
- iter != this->dataPtr->requestMsgs.end(); ++iter)
+ for (auto const &requestMsg : this->dataPtr->requestMsgs)
{
bool send = true;
- response.set_id((*iter).id());
- response.set_request((*iter).request());
+ response.set_id(requestMsg.id());
+ response.set_request(requestMsg.request());
response.set_response("success");
- if ((*iter).request() == "entity_list")
+ if (requestMsg.request() == "entity_list")
{
msgs::Model_V modelVMsg;
@@ -1370,14 +1360,14 @@ void World::ProcessRequestMsgs()
std::string *serializedData = response.mutable_serialized_data();
modelVMsg.SerializeToString(serializedData);
}
- else if ((*iter).request() == "entity_delete")
+ else if (requestMsg.request() == "entity_delete")
{
boost::mutex::scoped_lock lock2(this->dataPtr->entityDeleteMutex);
- this->dataPtr->deleteEntity.push_back((*iter).data());
+ this->dataPtr->deleteEntity.push_back(requestMsg.data());
}
- else if ((*iter).request() == "entity_info")
+ else if (requestMsg.request() == "entity_info")
{
- BasePtr entity = this->dataPtr->rootElement->GetByName((*iter).data());
+ BasePtr entity = this->dataPtr->rootElement->GetByName(requestMsg.data());
if (entity)
{
if (entity->HasType(Base::MODEL))
@@ -1428,7 +1418,7 @@ void World::ProcessRequestMsgs()
response.set_response("nonexistant");
}
}
- else if ((*iter).request() == "world_sdf")
+ else if (requestMsg.request() == "world_sdf")
{
msgs::GzString msg;
this->UpdateStateSDF();
@@ -1444,7 +1434,7 @@ void World::ProcessRequestMsgs()
msg.SerializeToString(serializedData);
response.set_type(msg.GetTypeName());
}
- else if ((*iter).request() == "scene_info")
+ else if (requestMsg.request() == "scene_info")
{
this->dataPtr->sceneMsg.clear_model();
this->BuildSceneMsg(this->dataPtr->sceneMsg, this->dataPtr->rootElement);
@@ -1453,7 +1443,7 @@ void World::ProcessRequestMsgs()
this->dataPtr->sceneMsg.SerializeToString(serializedData);
response.set_type(this->dataPtr->sceneMsg.GetTypeName());
}
- else if ((*iter).request() == "spherical_coordinates_info")
+ else if (requestMsg.request() == "spherical_coordinates_info")
{
msgs::SphericalCoordinates sphereCoordMsg;
msgs::Set(&sphereCoordMsg, *(this->dataPtr->sphericalCoordinates));
@@ -1478,22 +1468,20 @@ void World::ProcessRequestMsgs()
void World::ProcessModelMsgs()
{
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
- std::list<msgs::Model>::iterator iter;
- for (iter = this->dataPtr->modelMsgs.begin();
- iter != this->dataPtr->modelMsgs.end(); ++iter)
+ for (auto const &modelMsg : this->dataPtr->modelMsgs)
{
ModelPtr model;
- if ((*iter).has_id())
- model = this->GetModelById((*iter).id());
+ if (modelMsg.has_id())
+ model = this->GetModelById(modelMsg.id());
else
- model = this->GetModel((*iter).name());
+ model = this->GetModel(modelMsg.name());
if (!model)
gzerr << "Unable to find model["
- << (*iter).name() << "] Id[" << (*iter).id() << "]\n";
+ << modelMsg.name() << "] Id[" << modelMsg.id() << "]\n";
else
{
- model->ProcessMsg(*iter);
+ model->ProcessMsg(modelMsg);
// May 30, 2013: The following code was removed because it has a
// major performance impact when dragging complex object via the GUI.
@@ -1516,7 +1504,7 @@ void World::ProcessModelMsgs()
// }
// }
- this->dataPtr->modelPub->Publish(*iter);
+ this->dataPtr->modelPub->Publish(modelMsg);
}
}
@@ -1531,28 +1519,27 @@ void World::ProcessModelMsgs()
void World::ProcessFactoryMsgs()
{
std::list<sdf::ElementPtr> modelsToLoad;
- std::list<msgs::Factory>::iterator iter;
{
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
- for (iter = this->dataPtr->factoryMsgs.begin();
- iter != this->dataPtr->factoryMsgs.end(); ++iter)
+ for (auto const &factoryMsg : this->dataPtr->factoryMsgs)
{
this->dataPtr->factorySDF->root->ClearElements();
- if ((*iter).has_sdf() && !(*iter).sdf().empty())
+ if (factoryMsg.has_sdf() && !factoryMsg.sdf().empty())
{
// SDF Parsing happens here
- if (!sdf::readString((*iter).sdf(), this->dataPtr->factorySDF))
+ if (!sdf::readString(factoryMsg.sdf(), this->dataPtr->factorySDF))
{
- gzerr << "Unable to read sdf string[" << (*iter).sdf() << "]\n";
+ gzerr << "Unable to read sdf string[" << factoryMsg.sdf() << "]\n";
continue;
}
}
- else if ((*iter).has_sdf_filename() && !(*iter).sdf_filename().empty())
+ else if (factoryMsg.has_sdf_filename() &&
+ !factoryMsg.sdf_filename().empty())
{
std::string filename = common::ModelDatabase::Instance()->GetModelFile(
- (*iter).sdf_filename());
+ factoryMsg.sdf_filename());
if (!sdf::readFile(filename, this->dataPtr->factorySDF))
{
@@ -1560,12 +1547,12 @@ void World::ProcessFactoryMsgs()
continue;
}
}
- else if ((*iter).has_clone_model_name())
+ else if (factoryMsg.has_clone_model_name())
{
- ModelPtr model = this->GetModel((*iter).clone_model_name());
+ ModelPtr model = this->GetModel(factoryMsg.clone_model_name());
if (!model)
{
- gzerr << "Unable to clone model[" << (*iter).clone_model_name()
+ gzerr << "Unable to clone model[" << factoryMsg.clone_model_name()
<< "]. Model not found.\n";
continue;
}
@@ -1592,10 +1579,10 @@ void World::ProcessFactoryMsgs()
continue;
}
- if ((*iter).has_edit_name())
+ if (factoryMsg.has_edit_name())
{
BasePtr base =
- this->dataPtr->rootElement->GetByName((*iter).edit_name());
+ this->dataPtr->rootElement->GetByName(factoryMsg.edit_name());
if (base)
{
sdf::ElementPtr elem;
@@ -1649,8 +1636,8 @@ void World::ProcessFactoryMsgs()
elem->SetParent(this->dataPtr->sdf);
elem->GetParent()->InsertElement(elem);
- if ((*iter).has_pose())
- elem->GetElement("pose")->Set(msgs::Convert((*iter).pose()));
+ if (factoryMsg.has_pose())
+ elem->GetElement("pose")->Set(msgs::Convert(factoryMsg.pose()));
if (isActor)
{
@@ -1675,14 +1662,13 @@ void World::ProcessFactoryMsgs()
this->dataPtr->factoryMsgs.clear();
}
- for (std::list<sdf::ElementPtr>::iterator iter2 = modelsToLoad.begin();
- iter2 != modelsToLoad.end(); ++iter2)
+ for (auto const &elem : modelsToLoad)
{
try
{
boost::mutex::scoped_lock lock(this->dataPtr->factoryDeleteMutex);
- ModelPtr model = this->LoadModel(*iter2, this->dataPtr->rootElement);
+ ModelPtr model = this->LoadModel(elem, this->dataPtr->rootElement);
model->Init();
model->LoadPlugins();
}
@@ -1730,14 +1716,13 @@ void World::SetState(const WorldState &_state)
this->dataPtr->logRealTime = _state.GetRealTime();
const ModelState_M modelStates = _state.GetModelStates();
- for (ModelState_M::const_iterator iter = modelStates.begin();
- iter != modelStates.end(); ++iter)
+ for (auto const &modelState : modelStates)
{
- ModelPtr model = this->GetModel(iter->second.GetName());
+ ModelPtr model = this->GetModel(modelState.second.GetName());
if (model)
- model->SetState(iter->second);
+ model->SetState(modelState.second);
else
- gzerr << "Unable to find model[" << iter->second.GetName() << "]\n";
+ gzerr << "Unable to find model[" << modelState.second.GetName() << "]\n";
}
}
@@ -1780,20 +1765,18 @@ std::string World::StripWorldName(const std::string &_name) const
//////////////////////////////////////////////////
void World::EnableAllModels()
{
- for (Model_V::iterator iter = this->dataPtr->models.begin();
- iter != this->dataPtr->models.end(); ++iter)
+ for (auto &model : this->dataPtr->models)
{
- (*iter)->SetEnabled(true);
+ model->SetEnabled(true);
}
}
//////////////////////////////////////////////////
void World::DisableAllModels()
{
- for (Model_V::iterator iter = this->dataPtr->models.begin();
- iter != this->dataPtr->models.end(); ++iter)
+ for (auto &model : this->dataPtr->models)
{
- (*iter)->SetEnabled(false);
+ model->SetEnabled(false);
}
}
@@ -1828,11 +1811,11 @@ bool World::OnLog(std::ostringstream &_stream)
boost::mutex::scoped_lock lock(this->dataPtr->logBufferMutex);
this->dataPtr->currentStateBuffer ^= 1;
}
- for (std::deque<WorldState>::iterator iter =
- this->dataPtr->states[bufferIndex].begin();
- iter != this->dataPtr->states[bufferIndex].end(); ++iter)
+ for (auto const &worldState : this->dataPtr->states[bufferIndex])
{
- _stream << "<sdf version='" << SDF_VERSION << "'>" << *iter << "</sdf>";
+ _stream << "<sdf version='" << SDF_VERSION << "'>"
+ << worldState
+ << "</sdf>";
}
this->dataPtr->states[bufferIndex].clear();
@@ -1891,26 +1874,23 @@ void World::ProcessMessages()
if (!this->dataPtr->publishModelPoses.empty())
{
- for (std::set<ModelPtr>::iterator iter =
- this->dataPtr->publishModelPoses.begin();
- iter != this->dataPtr->publishModelPoses.end(); ++iter)
+ for (auto const &model : this->dataPtr->publishModelPoses)
{
msgs::Pose *poseMsg = msg.add_pose();
// Publish the model's relative pose
- poseMsg->set_name((*iter)->GetScopedName());
- poseMsg->set_id((*iter)->GetId());
- msgs::Set(poseMsg, (*iter)->GetRelativePose());
+ poseMsg->set_name(model->GetScopedName());
+ poseMsg->set_id(model->GetId());
+ msgs::Set(poseMsg, model->GetRelativePose());
// Publish each of the model's children relative poses
- Link_V links = (*iter)->GetLinks();
- for (Link_V::iterator linkIter = links.begin();
- linkIter != links.end(); ++linkIter)
+ Link_V links = model->GetLinks();
+ for (auto const &link : links)
{
poseMsg = msg.add_pose();
- poseMsg->set_name((*linkIter)->GetScopedName());
- poseMsg->set_id((*linkIter)->GetId());
- msgs::Set(poseMsg, (*linkIter)->GetRelativePose());
+ poseMsg->set_name(link->GetScopedName());
+ poseMsg->set_id(link->GetId());
+ msgs::Set(poseMsg, link->GetRelativePose());
}
}
@@ -2033,16 +2013,16 @@ void World::RemoveModel(const std::string &_name)
// Remove all the dirty poses from the delete entity.
{
- for (std::list<Entity*>::iterator iter2 = this->dataPtr->dirtyPoses.begin();
- iter2 != this->dataPtr->dirtyPoses.end();)
+ for (auto entity = this->dataPtr->dirtyPoses.begin();
+ entity != this->dataPtr->dirtyPoses.end(); ++entity)
{
- if ((*iter2)->GetName() == _name ||
- ((*iter2)->GetParent() && (*iter2)->GetParent()->GetName() == _name))
+ if ((*entity)->GetName() == _name ||
+ ((*entity)->GetParent() && (*entity)->GetParent()->GetName() == _name))
{
- this->dataPtr->dirtyPoses.erase(iter2++);
+ this->dataPtr->dirtyPoses.erase(entity++);
}
else
- ++iter2;
+ ++entity;
}
}
@@ -2083,12 +2063,12 @@ void World::RemoveModel(const std::string &_name)
this->dataPtr->rootElement->RemoveChild(_name);
- for (Model_V::iterator iter = this->dataPtr->models.begin();
- iter != this->dataPtr->models.end(); ++iter)
+ for (auto model = this->dataPtr->models.begin();
+ model != this->dataPtr->models.end(); ++model)
{
- if ((*iter)->GetName() == _name || (*iter)->GetScopedName() == _name)
+ if ((*model)->GetName() == _name || (*model)->GetScopedName() == _name)
{
- this->dataPtr->models.erase(iter);
+ this->dataPtr->models.erase(model);
break;
}
}
@@ -2097,13 +2077,12 @@ void World::RemoveModel(const std::string &_name)
// Cleanup the publishModelPoses list.
{
boost::recursive_mutex::scoped_lock lock2(*this->dataPtr->receiveMutex);
- for (std::set<ModelPtr>::iterator iter =
- this->dataPtr->publishModelPoses.begin();
- iter != this->dataPtr->publishModelPoses.end(); ++iter)
+ for (auto model = this->dataPtr->publishModelPoses.begin();
+ model != this->dataPtr->publishModelPoses.end(); ++model)
{
- if ((*iter)->GetName() == _name || (*iter)->GetScopedName() == _name)
+ if ((*model)->GetName() == _name || (*model)->GetScopedName() == _name)
{
- this->dataPtr->publishModelPoses.erase(iter);
+ this->dataPtr->publishModelPoses.erase(model);
break;
}
}
diff --git a/gazebo/physics/bullet/BulletHingeJoint.cc b/gazebo/physics/bullet/BulletHingeJoint.cc
index ec44959..4f2547e 100644
--- a/gazebo/physics/bullet/BulletHingeJoint.cc
+++ b/gazebo/physics/bullet/BulletHingeJoint.cc
@@ -394,6 +394,10 @@ bool BulletHingeJoint::SetParam(const std::string &_key,
return false;
}
}
+ else
+ {
+ return BulletJoint::SetParam(_key, _index, _value);
+ }
}
catch(const boost::bad_any_cast &e)
{
@@ -402,7 +406,7 @@ bool BulletHingeJoint::SetParam(const std::string &_key,
<< std::endl;
return false;
}
- return BulletJoint::SetParam(_key, _index, _value);
+ return true;
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/bullet/BulletJoint.cc b/gazebo/physics/bullet/BulletJoint.cc
index 6ca150f..5b99a85 100644
--- a/gazebo/physics/bullet/BulletJoint.cc
+++ b/gazebo/physics/bullet/BulletJoint.cc
@@ -538,19 +538,7 @@ bool BulletJoint::SetParam(const std::string &/*_key*/,
double BulletJoint::GetParam(const std::string &_key,
unsigned int _index)
{
- if (_key == "hi_stop")
- {
- return this->GetHighStop(_index).Radian();
- }
- else if (_key == "lo_stop")
- {
- return this->GetLowStop(_index).Radian();
- }
- gzerr << "GetParam unrecognized parameter ["
- << _key
- << "]"
- << std::endl;
- return 0;
+ return Joint::GetParam(_key, _index);
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/bullet/BulletLink.cc b/gazebo/physics/bullet/BulletLink.cc
index 731f6f4..f970462 100644
--- a/gazebo/physics/bullet/BulletLink.cc
+++ b/gazebo/physics/bullet/BulletLink.cc
@@ -147,6 +147,7 @@ void BulletLink::Init()
this->rigidLink->setUserPointer(this);
this->rigidLink->setCollisionFlags(this->rigidLink->getCollisionFlags() |
btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+ this->rigidLink->setFlags(BT_ENABLE_GYROPSCOPIC_FORCE);
/// \TODO: get friction from collision object
this->rigidLink->setAnisotropicFriction(btVector3(1, 1, 1),
diff --git a/gazebo/physics/bullet/BulletSliderJoint.cc b/gazebo/physics/bullet/BulletSliderJoint.cc
index 6348453..ba9d003 100644
--- a/gazebo/physics/bullet/BulletSliderJoint.cc
+++ b/gazebo/physics/bullet/BulletSliderJoint.cc
@@ -19,6 +19,7 @@
#include "gazebo/common/Exception.hh"
#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/World.hh"
#include "gazebo/physics/bullet/bullet_inc.h"
#include "gazebo/physics/bullet/BulletLink.hh"
#include "gazebo/physics/bullet/BulletPhysics.hh"
@@ -270,7 +271,7 @@ bool BulletSliderJoint::SetHighStop(unsigned int /*_index*/,
}
else
{
- gzerr << "bulletSlider not yet created.\n";
+ gzlog << "bulletSlider not yet created.\n";
return false;
}
}
@@ -287,7 +288,7 @@ bool BulletSliderJoint::SetLowStop(unsigned int /*_index*/,
}
else
{
- gzerr << "bulletSlider not yet created.\n";
+ gzlog << "bulletSlider not yet created.\n";
return false;
}
}
@@ -299,7 +300,7 @@ math::Angle BulletSliderJoint::GetHighStop(unsigned int /*_index*/)
if (this->bulletSlider)
result = this->bulletSlider->getUpperLinLimit();
else
- gzerr << "Joint must be created before getting high stop\n";
+ gzlog << "Joint must be created before getting high stop\n";
return result;
}
@@ -310,7 +311,7 @@ math::Angle BulletSliderJoint::GetLowStop(unsigned int /*_index*/)
if (this->bulletSlider)
result = this->bulletSlider->getLowerLinLimit();
else
- gzerr << "Joint must be created before getting low stop\n";
+ gzlog << "Joint must be created before getting low stop\n";
return result;
}
@@ -361,7 +362,7 @@ math::Angle BulletSliderJoint::GetAngleImpl(unsigned int _index) const
// if (this->bulletSlider)
// result = this->bulletSlider->getLinearPos();
// else
- // gzwarn << "bulletSlider does not exist, returning default position\n";
+ // gzlog << "bulletSlider does not exist, returning default position\n";
// Compute slider angle from gazebo's cached poses instead
math::Vector3 offset = this->GetWorldPose().pos
@@ -390,8 +391,16 @@ bool BulletSliderJoint::SetParam(const std::string &_key,
{
this->bulletSlider->setPoweredLinMotor(true);
this->bulletSlider->setTargetLinMotorVelocity(0.0);
+ // there is an upstream bug in bullet 2.82 and earlier
+ // the maxLinMotorForce parameter is inadvertently divided
+ // by timestep when attempting to compute an impulse in
+ // the bullet solver.
+ // https://github.com/bulletphysics/bullet3/pull/328
+ // As a workaround, multiply the desired friction
+ // parameter by dt^2 when setting
+ double dt = this->world->GetPhysicsEngine()->GetMaxStepSize();
this->bulletSlider->setMaxLinMotorForce(
- boost::any_cast<double>(_value));
+ dt*dt * boost::any_cast<double>(_value));
}
else
{
@@ -399,6 +408,10 @@ bool BulletSliderJoint::SetParam(const std::string &_key,
return false;
}
}
+ else
+ {
+ return BulletJoint::SetParam(_key, _index, _value);
+ }
}
catch(const boost::bad_any_cast &e)
{
@@ -407,7 +420,7 @@ bool BulletSliderJoint::SetParam(const std::string &_key,
<< std::endl;
return false;
}
- return BulletJoint::SetParam(_key, _index, _value);
+ return true;
}
//////////////////////////////////////////////////
@@ -423,7 +436,15 @@ double BulletSliderJoint::GetParam(const std::string &_key, unsigned int _index)
{
if (this->bulletSlider)
{
- return this->bulletSlider->getMaxLinMotorForce();
+ // there is an upstream bug in bullet 2.82 and earlier
+ // the maxLinMotorForce parameter is inadvertently divided
+ // by timestep when attempting to compute an impulse in
+ // the bullet solver.
+ // https://github.com/bulletphysics/bullet3/pull/328
+ // As a workaround, divide the desired friction
+ // parameter by dt^2 when getting
+ double dt = this->world->GetPhysicsEngine()->GetMaxStepSize();
+ return this->bulletSlider->getMaxLinMotorForce() / (dt*dt);
}
else
{
diff --git a/gazebo/physics/dart/DARTJoint.cc b/gazebo/physics/dart/DARTJoint.cc
index 197edd1..c1545b8 100644
--- a/gazebo/physics/dart/DARTJoint.cc
+++ b/gazebo/physics/dart/DARTJoint.cc
@@ -106,40 +106,6 @@ void DARTJoint::Init()
// We assume that the joint angles are all zero.
this->dtJoint->setTransformFromParentBodyNode(dtTransformParentLinkToJoint);
this->dtJoint->setTransformFromChildBodyNode(dtTransformChildLinkToJoint);
-
- //----------------------------------------------------------------------------
- // TODO: Currently, dampingCoefficient seems not to be initialized when
- // this joint is loaded. Therefore, we need below code...
- //----------------------------------------------------------------------------
- if (this->sdf->HasElement("axis"))
- {
- sdf::ElementPtr axisElem = this->sdf->GetElement("axis");
- if (axisElem->HasElement("dynamics"))
- {
- sdf::ElementPtr dynamicsElem = axisElem->GetElement("dynamics");
-
- if (dynamicsElem->HasElement("friction"))
- {
- sdf::ElementPtr frictionElem = dynamicsElem->GetElement("friction");
- gzlog << "joint friction not implemented in DART.\n";
- }
- }
- }
-
- if (this->sdf->HasElement("axis2"))
- {
- sdf::ElementPtr axisElem = this->sdf->GetElement("axis");
- if (axisElem->HasElement("dynamics"))
- {
- sdf::ElementPtr dynamicsElem = axisElem->GetElement("dynamics");
-
- if (dynamicsElem->HasElement("friction"))
- {
- sdf::ElementPtr frictionElem = dynamicsElem->GetElement("friction");
- gzlog << "joint friction not implemented in DART.\n";
- }
- }
- }
}
//////////////////////////////////////////////////
@@ -452,73 +418,59 @@ math::Vector3 DARTJoint::GetLinkTorque(unsigned int _index) const
//////////////////////////////////////////////////
bool DARTJoint::SetParam(const std::string &_key, unsigned int _index,
- const boost::any &_value)
+ const boost::any &_value)
{
- if (_key == "hi_stop")
+ // try because boost::any_cast can throw
+ try
{
- try
+ if (_key == "hi_stop")
{
this->SetHighStop(_index, boost::any_cast<double>(_value));
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "lo_stop")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ this->SetLowStop(_index, boost::any_cast<double>(_value));
}
- }
- else if (_key == "lo_stop")
- {
- try
+ else if (_key == "friction")
{
- this->SetLowStop(_index, boost::any_cast<double>(_value));
+ this->dtJoint->setCoulombFriction(_index,
+ boost::any_cast<double>(_value));
}
- catch(const boost::bad_any_cast &e)
+ else
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
+ gzerr << "Unable to handle joint attribute[" << _key << "]\n";
return false;
}
}
- else
+ catch(const boost::bad_any_cast &e)
{
- gzerr << "Unable to handle joint attribute[" << _key << "]\n";
+ gzerr << "SetParam(" << _key << ")"
+ << " boost any_cast error:" << e.what()
+ << std::endl;
return false;
}
+
return true;
}
//////////////////////////////////////////////////
-double DARTJoint::GetParam(const std::string& _key,
- unsigned int _index)
+double DARTJoint::GetParam(const std::string &_key, unsigned int _index)
{
- if (_key == "hi_stop")
- {
- try
- {
- return this->GetHighStop(_index).Radian();
- }
- catch(common::Exception &e)
- {
- gzerr << "GetParam error:" << e.GetErrorStr() << "\n";
- return 0;
- }
- }
- else if (_key == "lo_stop")
+ try
{
- try
- {
- return this->GetLowStop(_index).Radian();
- }
- catch(common::Exception &e)
+ if (_key == "friction")
{
- gzerr << "GetParam error:" << e.GetErrorStr() << "\n";
- return 0;
+ return this->dtJoint->getCoulombFriction(_index);
}
}
- else
+ catch(const common::Exception &e)
{
- gzerr << "Unable to get joint attribute[" << _key << "]\n";
+ gzerr << "GetParam(" << _key << ") error:"
+ << e.GetErrorStr()
+ << std::endl;
return 0;
}
+ return Joint::GetParam(_key, _index);
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/dart/DARTLink.cc b/gazebo/physics/dart/DARTLink.cc
index 3d78c60..d78160a 100644
--- a/gazebo/physics/dart/DARTLink.cc
+++ b/gazebo/physics/dart/DARTLink.cc
@@ -247,9 +247,28 @@ void DARTLink::OnPoseChange()
P = this->dtBodyNode->getParentBodyNode()->getTransform();
Eigen::Isometry3d Q = T1.inverse() * P.inverse() * W * InvT2;
- // Set generalized coordinate and update the transformations only
- freeJoint->setPositions(dart::math::logMap(Q));
+
+ // Convert homogeneous transformation matrix to 6-dimensional generalized
+ // coordinates. There are several ways of conversions. Here is the way of
+ // DART. The orientation part is converted by using logarithm map, which
+ // maps SO(3) to so(3), and it takes the first three components of the
+ // generalized coordinates. On the other hand, the position part just takes
+ // the last three components of the generalized coordinates without any
+ // conversion.
+ Eigen::Vector6d q;
+ q.head<3>() = dart::math::logMap(Q.linear());
+ q.tail<3>() = Q.translation();
+ freeJoint->setPositions(q);
+ // TODO: The above 4 lines will be reduced to single line as:
+ // freeJoint->setPositions(FreeJoint::convertToPositions(Q));
+ // after the following PR is merged:
+ // https://github.com/dartsim/dart/pull/322
+
+ // Update all the transformations of the links in the parent model.
freeJoint->getSkeleton()->computeForwardKinematics(true, false, false);
+ // TODO: This kinematic updating will be done automatically after pull
+ // request (https://github.com/dartsim/dart/pull/319) is merged so that
+ // we don't need this line anymore.
}
else
{
diff --git a/gazebo/physics/dart/dart_inc.h b/gazebo/physics/dart/dart_inc.h
index 0c6b0a2..03da4c0 100644
--- a/gazebo/physics/dart/dart_inc.h
+++ b/gazebo/physics/dart/dart_inc.h
@@ -21,43 +21,12 @@
// This disables warning messages for ODE
#pragma GCC system_header
-#include <dart/math/Helpers.h>
-#include <dart/math/Geometry.h>
-
-#include <dart/collision/CollisionDetector.h>
-#include <dart/collision/CollisionNode.h>
-#include <dart/collision/dart/DARTCollisionDetector.h>
-#include <dart/collision/fcl_mesh/FCLMeshCollisionDetector.h>
-
-#include <dart/integration/Integrator.h>
-#include <dart/integration/EulerIntegrator.h>
-#include <dart/integration/RK4Integrator.h>
-
-#include <dart/dynamics/BallJoint.h>
-#include <dart/dynamics/BodyNode.h>
-#include <dart/dynamics/BoxShape.h>
-#include <dart/dynamics/CylinderShape.h>
-#include <dart/dynamics/EllipsoidShape.h>
-#include <dart/dynamics/FreeJoint.h>
-#include <dart/dynamics/Joint.h>
-#include <dart/dynamics/MeshShape.h>
-#include <dart/dynamics/PointMass.h>
-#include <dart/dynamics/PrismaticJoint.h>
-#include <dart/dynamics/RevoluteJoint.h>
-#include <dart/dynamics/Shape.h>
-#include <dart/dynamics/Skeleton.h>
-#include <dart/dynamics/ScrewJoint.h>
-#include <dart/dynamics/UniversalJoint.h>
-#include <dart/dynamics/WeldJoint.h>
-#include <dart/dynamics/SoftBodyNode.h>
-#include <dart/dynamics/SoftMeshShape.h>
-
-#include <dart/constraint/Constraint.h>
-#include <dart/constraint/ConstraintSolver.h>
-#include <dart/constraint/ContactConstraint.h>
-#include <dart/constraint/JointLimitConstraint.h>
-#include <dart/constraint/WeldJointConstraint.h>
-
-#include <dart/simulation/World.h>
+#include <dart/common/common.h>
+#include <dart/math/math.h>
+#include <dart/collision/collision.h>
+#include <dart/integration/integration.h>
+#include <dart/dynamics/dynamics.h>
+#include <dart/constraint/constraint.h>
+#include <dart/simulation/simulation.h>
#endif
diff --git a/gazebo/physics/simbody/SimbodyJoint.cc b/gazebo/physics/simbody/SimbodyJoint.cc
index 0740808..8a67f3e 100644
--- a/gazebo/physics/simbody/SimbodyJoint.cc
+++ b/gazebo/physics/simbody/SimbodyJoint.cc
@@ -479,11 +479,9 @@ bool SimbodyJoint::SetParam(const std::string &/*_key*/,
}
//////////////////////////////////////////////////
-double SimbodyJoint::GetParam(const std::string &/*_key*/,
- unsigned int /*_index*/)
+double SimbodyJoint::GetParam(const std::string &_key, unsigned int _index)
{
- gzerr << "Not implement in Simbody\n";
- return 0;
+ return Joint::GetParam(_key, _index);
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/simbody/SimbodyPhysics.cc b/gazebo/physics/simbody/SimbodyPhysics.cc
index daba003..9fb283f 100644
--- a/gazebo/physics/simbody/SimbodyPhysics.cc
+++ b/gazebo/physics/simbody/SimbodyPhysics.cc
@@ -162,13 +162,13 @@ void SimbodyPhysics::Load(sdf::ElementPtr _sdf)
simbodyContactElem->Get<double>("viscous_friction");
// below are not used yet, but should work it into the system
- this->contactMaterialViscousFriction =
- simbodyContactElem->Get<double>("plastic_coef_restitution");
this->contactMaterialPlasticCoefRestitution =
- simbodyContactElem->Get<double>("plastic_impact_velocity");
+ simbodyContactElem->Get<double>("plastic_coef_restitution");
this->contactMaterialPlasticImpactVelocity =
- simbodyContactElem->Get<double>("override_impact_capture_velocity");
+ simbodyContactElem->Get<double>("plastic_impact_velocity");
this->contactImpactCaptureVelocity =
+ simbodyContactElem->Get<double>("override_impact_capture_velocity");
+ this->contactStictionTransitionVelocity =
simbodyContactElem->Get<double>("override_stiction_transition_velocity");
}
diff --git a/gazebo/physics/simbody/SimbodyScrewJoint.cc b/gazebo/physics/simbody/SimbodyScrewJoint.cc
index 477ae95..95977a8 100644
--- a/gazebo/physics/simbody/SimbodyScrewJoint.cc
+++ b/gazebo/physics/simbody/SimbodyScrewJoint.cc
@@ -256,7 +256,7 @@ double SimbodyScrewJoint::GetThreadPitch(unsigned int /*_index*/)
//////////////////////////////////////////////////
double SimbodyScrewJoint::GetThreadPitch()
{
- if (this->physicsInitialized &&
+ if (!this->mobod.isEmptyHandle() && this->physicsInitialized &&
this->simbodyPhysics->simbodyPhysicsInitialized)
{
// downcast mobod to screw mobod first
@@ -315,6 +315,14 @@ bool SimbodyScrewJoint::SetHighStop(
{
if (this->physicsInitialized)
{
+ // check if limitForce is initialized
+ if (this->limitForce[_index].isEmptyHandle())
+ {
+ gzerr << "child link is NULL, force element not initialized, "
+ << "SetHighStop failed. Please file a report on issue tracker.\n";
+ return false;
+ }
+
if (_index == 0)
{
// angular limit is specified
@@ -384,6 +392,14 @@ bool SimbodyScrewJoint::SetLowStop(
{
if (this->physicsInitialized)
{
+ // check if limitForce is initialized
+ if (this->limitForce[_index].isEmptyHandle())
+ {
+ gzerr << "child link is NULL, force element not initialized, "
+ << "SetHighStop failed. Please file a report on issue tracker.\n";
+ return false;
+ }
+
if (_index == 0)
{
// angular limit is specified
diff --git a/gazebo/rendering/Camera.cc b/gazebo/rendering/Camera.cc
index 7bb2483..4525b90 100644
--- a/gazebo/rendering/Camera.cc
+++ b/gazebo/rendering/Camera.cc
@@ -480,7 +480,10 @@ void Camera::PostRender()
{
this->ReadPixelBuffer();
- this->lastRenderWallTime = common::Time::GetWallTime();
+ // Only record last render time if data was actually generated
+ // (If a frame was rendered).
+ if (this->newData)
+ this->lastRenderWallTime = common::Time::GetWallTime();
if (this->newData && (this->captureData || this->captureDataOnce))
{
diff --git a/gazebo/rendering/GpuLaser_TEST.cc b/gazebo/rendering/GpuLaser_TEST.cc
index fdb8481..730e2b0 100644
--- a/gazebo/rendering/GpuLaser_TEST.cc
+++ b/gazebo/rendering/GpuLaser_TEST.cc
@@ -22,6 +22,7 @@
#include "gazebo/rendering/GpuLaser.hh"
#include "test/ServerFixture.hh"
+using namespace gazebo;
class GpuLaser_TEST : public ServerFixture
{
};
diff --git a/gazebo/rendering/Heightmap.cc b/gazebo/rendering/Heightmap.cc
index 8546d00..d77ebc3 100644
--- a/gazebo/rendering/Heightmap.cc
+++ b/gazebo/rendering/Heightmap.cc
@@ -44,6 +44,13 @@ const double Heightmap::holdRadiusFactor = 1.15;
const boost::filesystem::path Heightmap::pagingDirname = "paging";
const boost::filesystem::path Heightmap::hashFilename = "gzterrain.SHA1";
+static std::string glslVersion = "130";
+static std::string vpInStr = "in";
+static std::string vpOutStr = "out";
+static std::string fpInStr = "in";
+static std::string fpOutStr = "out";
+static std::string textureStr = "texture";
+
//////////////////////////////////////////////////
Heightmap::Heightmap(ScenePtr _scene)
{
@@ -290,6 +297,26 @@ void Heightmap::Load()
if (this->terrainGlobals != NULL)
return;
+ const Ogre::RenderSystemCapabilities *capabilities;
+ Ogre::RenderSystemCapabilities::ShaderProfiles profiles;
+ Ogre::RenderSystemCapabilities::ShaderProfiles::const_iterator iter;
+
+ capabilities =
+ Ogre::Root::getSingleton().getRenderSystem()->getCapabilities();
+ Ogre::DriverVersion glVersion;
+ glVersion.build = 0;
+ glVersion.major = 3;
+ glVersion.minor = 0;
+ glVersion.release = 0;
+ if (capabilities->isDriverOlderThanVersion(glVersion))
+ {
+ glslVersion = "120";
+ vpInStr = "attribute";
+ vpOutStr = "varying";
+ fpInStr = "varying";
+ textureStr = "texture2D";
+ }
+
// The terraingGroup is composed by a number of terrains (1 by default)
int nTerrains = 1;
@@ -607,7 +634,7 @@ bool Heightmap::InitBlendMaps(Ogre::Terrain *_terrain)
{
if (!_terrain)
{
- std::cerr << "Invalid terrain\n";
+ gzerr << "Invalid terrain\n";
return false;
}
@@ -1401,7 +1428,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
{
bool compression = false;
- _outStream << "#version 130\n\n";
+ _outStream << "#version " << glslVersion << "\n\n";
#if OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 8
compression = _terrain->_getUseVertexCompression() &&
@@ -1411,21 +1438,21 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
{
// The parameter "in vec4 vertex;" is automatically bound by OGRE.
// The parameter "in vec4 uv0'" is automatically bound by OGRE.
- _outStream << "in vec4 vertex;\n"
- << "in vec4 uv0;\n";
+ _outStream << vpInStr << " vec4 vertex;\n"
+ << vpInStr << " vec4 uv0;\n";
}
else
#endif
{
// The parameter "in vec4 vertex;" is automatically bound by OGRE.
// The parameter "in vec4 uv0'" is automatically bound by OGRE.
- _outStream << "in vec4 vertex;\n"
- << "in vec4 uv0;\n";
+ _outStream << vpInStr << " vec4 vertex;\n"
+ << vpInStr << " vec4 uv0;\n";
}
if (_tt != RENDER_COMPOSITE_MAP)
// The parameter "in vec4 uv1'" is automatically bound by OGRE.
- _outStream << "in vec4 uv1;\n";
+ _outStream << vpInStr << " vec4 uv1;\n";
_outStream <<
"uniform mat4 worldMatrix;\n"
@@ -1454,10 +1481,10 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
_outStream << "uniform vec4 uvMul" << i << ";\n";
_outStream <<
- "out vec4 position;\n";
+ vpOutStr << " vec4 position;\n";
unsigned int texCoordSet = 1;
- _outStream << "out vec4 uvMisc;\n";
+ _outStream << vpOutStr << " vec4 uvMisc;\n";
// layer UV's premultiplied, packed as xy/zw
unsigned int numUVSets = numLayers / 2;
@@ -1469,13 +1496,13 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
{
for (unsigned int i = 0; i < numUVSets; ++i)
{
- _outStream << "out vec4 layerUV" << i << ";\n";
+ _outStream << vpOutStr << " vec4 layerUV" << i << ";\n";
}
}
if (_prof->getParent()->getDebugLevel() && _tt != RENDER_COMPOSITE_MAP)
{
- _outStream << "out vec2 lodInfo;\n";
+ _outStream << vpOutStr << " vec2 lodInfo;\n";
}
bool fog = _terrain->getSceneManager()->getFogMode() != Ogre::FOG_NONE &&
@@ -1485,7 +1512,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
{
_outStream <<
"uniform vec4 fogParams;\n"
- "out float fogVal;\n";
+ << vpOutStr << " float fogVal;\n";
}
if (_prof->isShadowingEnabled(_tt, _terrain))
@@ -1723,7 +1750,7 @@ generateVpDynamicShadowsParams(unsigned int _texCoord, const SM2Profile *_prof,
for (unsigned int i = 0; i < numTextures; ++i)
{
- _outStream << "out vec4 lightSpacePos" << i << ";\n"
+ _outStream << vpOutStr << " vec4 lightSpacePos" << i << ";\n"
<< "uniform mat4 texViewProjMatrix" << i << ";\n";
// Don't add depth range params
@@ -1742,7 +1769,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
TechniqueType _tt, Ogre::StringUtil::StrStreamType &_outStream)
{
- _outStream << "#version 130\n\n";
+ _outStream << "#version " << glslVersion << "\n\n";
_outStream <<
"vec4 expand(vec4 v)\n"
@@ -1761,10 +1788,10 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
this->generateFpDynamicShadowsHelpers(_prof, _terrain, _tt, _outStream);
_outStream <<
- "in vec4 position;\n";
+ fpInStr << " vec4 position;\n";
Ogre::uint texCoordSet = 1;
- _outStream << "in vec4 uvMisc;\n";
+ _outStream << fpInStr << " vec4 uvMisc;\n";
// UV's premultiplied, packed as xy/zw
Ogre::uint maxLayers = _prof->getMaxLayers(_terrain);
@@ -1784,13 +1811,13 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
for (Ogre::uint i = 0; i < numUVSets; ++i)
{
_outStream <<
- "in vec4 layerUV" << i << ";\n";
+ fpInStr << " vec4 layerUV" << i << ";\n";
}
}
if (_prof->getParent()->getDebugLevel() && _tt != RENDER_COMPOSITE_MAP)
{
- _outStream << "in vec2 lodInfo;\n";
+ _outStream << fpInStr << " vec2 lodInfo;\n";
}
bool fog = _terrain->getSceneManager()->getFogMode() != Ogre::FOG_NONE &&
@@ -1800,7 +1827,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
{
_outStream <<
"uniform vec3 fogColour;\n"
- "in float fogVal;\n";
+ << fpInStr << " float fogVal;\n";
}
Ogre::uint currentSamplerIdx = 0;
@@ -1864,19 +1891,25 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
"Try reducing the number of layers.", __FUNCTION__);
}
- _outStream << "out vec4 outputCol;\n";
+ std::string outputColTypeStr = "vec4";
+ if (glslVersion != "120")
+ {
+ _outStream << "out vec4 outputCol;\n";
+ outputColTypeStr = "";
+ }
_outStream <<
"void main()\n"
"{\n"
" float shadow = 1.0;\n"
" vec2 uv = uvMisc.xy;\n"
- " outputCol = vec4(0.0, 0.0, 0.0, 1.0);\n";
+ " " << outputColTypeStr << " outputCol = vec4(0.0, 0.0, 0.0, 1.0);\n";
if (_tt != LOW_LOD)
{
// global normal
- _outStream << " vec3 normal = expand(texture(globalNormal, uv)).xyz;\n";
+ _outStream << " vec3 normal = expand("
+ << textureStr << "(globalNormal, uv)).xyz;\n";
}
_outStream <<
@@ -1892,7 +1925,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
{
// we just do a single calculation from composite map
_outStream <<
- " vec4 composite = texture(compositeMap, uv);\n"
+ " vec4 composite = " << textureStr << "(compositeMap, uv);\n"
" diffuse = composite.xyz;\n";
// TODO - specular; we'll need normals for this!
}
@@ -1902,7 +1935,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
for (Ogre::uint i = 0; i < numBlendTextures; ++i)
{
_outStream << " vec4 blendTexVal" << i
- << " = texture(blendTex" << i << ", uv);\n";
+ << " = " << textureStr << "(blendTex" << i << ", uv);\n";
}
if (_prof->isLayerNormalMappingEnabled())
@@ -1984,7 +2017,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsParams(
for (Ogre::uint i = 0; i < numTextures; ++i)
{
_outStream <<
- "in vec4 lightSpacePos" << i << ";\n" <<
+ "varying vec4 lightSpacePos" << i << ";\n" <<
"uniform sampler2D shadowMap" << i << ";\n";
*_sampler = *_sampler + 1;
@@ -2026,15 +2059,15 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpLayer(
if (_prof->isLayerParallaxMappingEnabled() && _tt != RENDER_COMPOSITE_MAP)
{
// modify UV - note we have to sample an extra time
- _outStream << " displacement = texture(normtex" << _layer
+ _outStream << " displacement = " << textureStr << "(normtex" << _layer
<< ", uv" << _layer << ").w\n"
" * scaleBiasSpecular.x + scaleBiasSpecular.y;\n";
_outStream << " uv" << _layer << " += TSeyeDir.xy * displacement;\n";
}
// access TS normal map
- _outStream << " TSnormal = expand(texture(normtex" << _layer << ", uv"
- << _layer << ")).xyz;\n";
+ _outStream << " TSnormal = expand(" << textureStr << "(normtex"
+ << _layer << ", uv" << _layer << ")).xyz;\n";
_outStream << " TShalfAngle = normalize(TSlightDir + TSeyeDir);\n";
_outStream << " litResLayer = lit(dot(TSlightDir, TSnormal), "
@@ -2049,7 +2082,8 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpLayer(
// sample diffuse texture
_outStream << " vec4 diffuseSpecTex" << _layer
- << " = texture(difftex" << _layer << ", uv" << _layer << ");\n";
+ << " = " << textureStr << "(difftex" << _layer << ", uv" << _layer
+ << ");\n";
// apply to common
if (!_layer)
@@ -2099,13 +2133,14 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpFooter(
_prof->isGlobalColourMapEnabled())
{
// sample colour map and apply to diffuse
- _outStream << " diffuse *= texture(globalColourMap, uv).xyz;\n";
+ _outStream << " diffuse *= " << textureStr
+ << "(globalColourMap, uv).xyz;\n";
}
if (_prof->isLightmapEnabled())
{
// sample lightmap
- _outStream << " shadow = texture(lightMap, uv).x;\n";
+ _outStream << " shadow = " << textureStr << "(lightMap, uv).x;\n";
}
if (_prof->isShadowingEnabled(_tt, _terrain))
@@ -2146,6 +2181,8 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpFooter(
_outStream << " outputCol.xyz = mix(outputCol.xyz, fogColour, fogVal);\n";
}
+ if (glslVersion == "120")
+ _outStream << " gl_FragColor = outputCol;\n";
// Final return
_outStream << "\n}\n";
@@ -2187,11 +2224,19 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsHelpers(
" vec4 newUV = offsetSample(uv, vec2(x, y), invShadowMapSize);\n"
" // manually project and assign derivatives\n"
" // to avoid gradient issues inside loops\n"
- " newUV = newUV / newUV.w;\n"
+ " newUV = newUV / newUV.w;\n";
// The following line used to be:
- // " float depth = tex2d(shadowMap, newUV.xy, 1.0, 1.0).x;\n"
- " float depth = textureGrad(shadowMap, newUV.xy, "
- " vec2(1.0, 1.0), vec2(1.0, 1.0)).x;\n"
+ // " float depth = tex2d(shadowMap, newUV.xy).x;\n"
+ if (glslVersion == "120")
+ _outStream <<
+ " float depth = texture2D(shadowMap, newUV.xy).x;\n";
+ else
+ {
+ _outStream <<
+ " float depth = textureGrad(shadowMap, newUV.xy, "
+ " vec2(1.0, 1.0), vec2(1.0, 1.0)).x;\n";
+ }
+ _outStream <<
// " if (depth >= 1.0 || depth >= uv.z)\n"
" if (depth >= 1.0 || depth >= newUV.z)\n"
" shadow += 1.0;\n"
@@ -2205,7 +2250,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsHelpers(
_outStream <<
"float calcSimpleShadow(sampler2D shadowMap, vec4 shadowMapPos)\n"
"{\n"
- " return textureProj(shadowMap, shadowMapPos).x;\n"
+ " return " << textureStr << "Proj(shadowMap, shadowMapPos).x;\n"
"}\n";
}
diff --git a/gazebo/rendering/JointVisual_TEST.cc b/gazebo/rendering/JointVisual_TEST.cc
index abd6d0b..9c1a07f 100644
--- a/gazebo/rendering/JointVisual_TEST.cc
+++ b/gazebo/rendering/JointVisual_TEST.cc
@@ -22,6 +22,7 @@
#include "gazebo/rendering/JointVisual.hh"
#include "test/ServerFixture.hh"
+using namespace gazebo;
class JointVisual_TEST : public ServerFixture
{
};
diff --git a/gazebo/rendering/SonarVisual_TEST.cc b/gazebo/rendering/SonarVisual_TEST.cc
index 9b6ee24..c4003a9 100644
--- a/gazebo/rendering/SonarVisual_TEST.cc
+++ b/gazebo/rendering/SonarVisual_TEST.cc
@@ -22,6 +22,7 @@
#include "gazebo/rendering/SonarVisual.hh"
#include "test/ServerFixture.hh"
+using namespace gazebo;
class SonarVisual_TEST : public ServerFixture
{
};
diff --git a/gazebo/rendering/TransmitterVisual_TEST.cc b/gazebo/rendering/TransmitterVisual_TEST.cc
index 67fd50d..0011a10 100644
--- a/gazebo/rendering/TransmitterVisual_TEST.cc
+++ b/gazebo/rendering/TransmitterVisual_TEST.cc
@@ -22,6 +22,7 @@
#include "gazebo/rendering/TransmitterVisual.hh"
#include "test/ServerFixture.hh"
+using namespace gazebo;
class TransmitterVisual_TEST : public ServerFixture
{
};
diff --git a/gazebo/rendering/UserCamera.cc b/gazebo/rendering/UserCamera.cc
index 40b3759..afc8fbb 100644
--- a/gazebo/rendering/UserCamera.cc
+++ b/gazebo/rendering/UserCamera.cc
@@ -46,8 +46,9 @@ UserCamera::UserCamera(const std::string &_name, ScenePtr _scene)
this->dataPtr->joystickButtonToggleLast = false;
this->dataPtr->joyPoseControl = true;
- // Set default UserCamera render rate to 30Hz
- this->SetRenderRate(30.0);
+ // We want a render rate of 60Hz. Setting the target to 70Hz to account
+ // for some slop
+ this->SetRenderRate(70.0);
this->SetUseSDFPose(false);
}
@@ -206,9 +207,6 @@ void UserCamera::Fini()
//////////////////////////////////////////////////
void UserCamera::HandleMouseEvent(const common::MouseEvent &_evt)
{
- if (this->dataPtr->selectionBuffer)
- this->dataPtr->selectionBuffer->Update();
-
// DEBUG: this->dataPtr->selectionBuffer->ShowOverlay(true);
// Don't update the camera if it's being animated.
@@ -380,8 +378,15 @@ void UserCamera::SetViewportDimensions(float /*x_*/, float /*y_*/,
//////////////////////////////////////////////////
float UserCamera::GetAvgFPS() const
{
- return RenderEngine::Instance()->GetWindowManager()->GetAvgFPS(
- this->windowId);
+ float avgFPS = 0;
+
+ if (this->renderTarget)
+ {
+ float lastFPS, bestFPS, worstFPS = 0;
+ this->renderTarget->getStatistics(lastFPS, avgFPS, bestFPS, worstFPS);
+ }
+
+ return avgFPS;
}
//////////////////////////////////////////////////
@@ -540,9 +545,6 @@ VisualPtr UserCamera::GetVisual(const math::Vector2i &_mousePos,
if (!this->dataPtr->selectionBuffer)
return result;
- // Update the selection buffer
- this->dataPtr->selectionBuffer->Update();
-
Ogre::Entity *entity =
this->dataPtr->selectionBuffer->OnSelectionClick(_mousePos.x, _mousePos.y);
diff --git a/gazebo/rendering/selection_buffer/SelectionBuffer.cc b/gazebo/rendering/selection_buffer/SelectionBuffer.cc
index 4caaca8..9262902 100644
--- a/gazebo/rendering/selection_buffer/SelectionBuffer.cc
+++ b/gazebo/rendering/selection_buffer/SelectionBuffer.cc
@@ -27,7 +27,7 @@ using namespace rendering;
/////////////////////////////////////////////////
SelectionBuffer::SelectionBuffer(const std::string &_cameraName,
Ogre::SceneManager *_mgr, Ogre::RenderTarget *_renderTarget)
-: sceneMgr(_mgr), renderTarget(_renderTarget),
+: sceneMgr(_mgr), renderTarget(_renderTarget), renderTexture(0),
buffer(0), pixelBox(0)
{
this->camera = this->sceneMgr->getCamera(_cameraName);
diff --git a/gazebo/sensors/ContactSensor.cc b/gazebo/sensors/ContactSensor.cc
index 71594d3..8f0592f 100644
--- a/gazebo/sensors/ContactSensor.cc
+++ b/gazebo/sensors/ContactSensor.cc
@@ -92,6 +92,7 @@ void ContactSensor::Load(const std::string &_worldName)
std::string entityName =
this->world->GetEntity(this->parentName)->GetScopedName();
+ std::string filterName = entityName + "::" + this->GetName();
// Get all the collision elements
while (collisionElem)
@@ -112,7 +113,7 @@ void ContactSensor::Load(const std::string &_worldName)
// this sensor
physics::ContactManager *mgr =
this->world->GetPhysicsEngine()->GetContactManager();
- std::string topic = mgr->CreateFilter(entityName, this->collisions);
+ std::string topic = mgr->CreateFilter(filterName, this->collisions);
if (!this->contactSub)
{
this->contactSub = this->node->Subscribe(topic,
@@ -207,10 +208,11 @@ void ContactSensor::Fini()
{
std::string entityName =
this->world->GetEntity(this->parentName)->GetScopedName();
+ std::string filterName = entityName + "::" + this->GetName();
physics::ContactManager *mgr =
this->world->GetPhysicsEngine()->GetContactManager();
- mgr->RemoveFilter(entityName);
+ mgr->RemoveFilter(filterName);
}
this->contactSub.reset();
diff --git a/gazebo/sensors/ImuSensor.cc b/gazebo/sensors/ImuSensor.cc
index 1da46a4..36255b0 100644
--- a/gazebo/sensors/ImuSensor.cc
+++ b/gazebo/sensors/ImuSensor.cc
@@ -243,6 +243,8 @@ bool ImuSensor::UpdateImpl(bool /*_force*/)
double dt = (timestamp - this->lastMeasurementTime).Double();
+ this->lastMeasurementTime = timestamp;
+
if (dt > 0.0)
{
boost::mutex::scoped_lock lock(this->mutex);
@@ -283,8 +285,6 @@ bool ImuSensor::UpdateImpl(bool /*_force*/)
this->lastLinearVel = imuWorldLinearVel;
- this->lastMeasurementTime = timestamp;
-
if (this->noiseActive)
{
switch (this->noiseType)
diff --git a/gazebo/sensors/Sensor_TEST.cc b/gazebo/sensors/Sensor_TEST.cc
index 888fe0a..59d3eb3 100644
--- a/gazebo/sensors/Sensor_TEST.cc
+++ b/gazebo/sensors/Sensor_TEST.cc
@@ -25,7 +25,8 @@ class Sensor_TEST : public ServerFixture
{
};
-boost::condition_variable g_countCondition;
+boost::condition_variable g_hokuyoCountCondition;
+boost::condition_variable g_imuCountCondition;
// global variable and callback for tracking hokuyo sensor messages
unsigned int g_hokuyoMsgCount;
@@ -33,7 +34,16 @@ void ReceiveHokuyoMsg(ConstLaserScanStampedPtr &/*_msg*/)
{
g_hokuyoMsgCount++;
if (g_hokuyoMsgCount >= 20)
- g_countCondition.notify_one();
+ g_hokuyoCountCondition.notify_one();
+}
+
+// global variable and callback for tracking imu sensor messages
+unsigned int g_imuMsgCount;
+void ReceiveImuMsg(ConstLaserScanStampedPtr &/*_msg*/)
+{
+ g_imuMsgCount++;
+ if (g_imuMsgCount >= 20)
+ g_imuCountCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -47,7 +57,7 @@ TEST_F(Sensor_TEST, UpdateAfterReset)
ASSERT_TRUE(world != NULL);
unsigned int i;
- double updateRate, now, then;
+ double updateRateHokuyo, updateRateImu, now, then;
// get the sensor manager
sensors::SensorManager *mgr = sensors::SensorManager::Instance();
@@ -58,50 +68,74 @@ TEST_F(Sensor_TEST, UpdateAfterReset)
sensor = mgr->GetSensor("default::hokuyo::link::laser");
ASSERT_TRUE(sensor != NULL);
+ sensors::SensorPtr imuSensor;
+ imuSensor = mgr->GetSensor("default::box_model::box_link::box_imu_sensor");
+ ASSERT_TRUE(imuSensor != NULL);
+
// set update rate to 30 Hz
- updateRate = 30.0;
- sensor->SetUpdateRate(updateRate);
+ updateRateHokuyo = 30.0;
+ updateRateImu = 1000.0;
+ sensor->SetUpdateRate(updateRateHokuyo);
+ imuSensor->SetUpdateRate(updateRateImu);
gzdbg << sensor->GetScopedName() << " loaded with update rate of "
- << sensor->GetUpdateRate() << " Hz\n";
+ << sensor->GetUpdateRate() << " Hz"
+ << std::endl;
+ gzdbg << imuSensor->GetScopedName() << " loaded with update rate of "
+ << imuSensor->GetUpdateRate() << " Hz"
+ << std::endl;
g_hokuyoMsgCount = 0;
+ g_imuMsgCount = 0;
- // Subscribe to hokuyo laser scan messages
+ // Subscribe to sensor messages
transport::NodePtr node = transport::NodePtr(new transport::Node());
node->Init();
- transport::SubscriberPtr sceneSub = node->Subscribe(
+ transport::SubscriberPtr laserSub = node->Subscribe(
"~/hokuyo/link/laser/scan", &ReceiveHokuyoMsg);
+ transport::SubscriberPtr imuSub = node->Subscribe(
+ "~/box_model/box_link/box_imu_sensor/imu", &ReceiveImuMsg);
// Wait for messages to arrive
{
boost::mutex countMutex;
boost::mutex::scoped_lock lock(countMutex);
- g_countCondition.wait(lock);
+ g_hokuyoCountCondition.wait(lock);
+ g_imuCountCondition.wait(lock);
}
unsigned int hokuyoMsgCount = g_hokuyoMsgCount;
+ unsigned int imuMsgCount = g_imuMsgCount;
now = world->GetSimTime().Double();
- gzdbg << "counted " << hokuyoMsgCount << " messages in "
+ gzdbg << "counted " << hokuyoMsgCount << " hokuyo messages in "
+ << now << " seconds\n";
+ gzdbg << "counted " << imuMsgCount << " imu messages in "
<< now << " seconds\n";
// Expect at least 50% of specified update rate
EXPECT_GT(static_cast<double>(hokuyoMsgCount),
- updateRate*now * 0.5);
+ updateRateHokuyo*now * 0.5);
+ EXPECT_GT(static_cast<double>(imuMsgCount),
+ updateRateImu*now * 0.5);
// Wait another 1.5 seconds
for (i = 0; i < 15; ++i)
common::Time::MSleep(100);
hokuyoMsgCount = g_hokuyoMsgCount;
+ imuMsgCount = g_imuMsgCount;
now = world->GetSimTime().Double();
- gzdbg << "counted " << hokuyoMsgCount << " messages in "
+ gzdbg << "counted " << hokuyoMsgCount << " hokuyo messages in "
+ << now << " seconds\n";
+ gzdbg << "counted " << imuMsgCount << " imu messages in "
<< now << " seconds\n";
// Expect at least 50% of specified update rate
EXPECT_GT(static_cast<double>(hokuyoMsgCount),
- updateRate*now * 0.5);
+ updateRateHokuyo*now * 0.5);
+ EXPECT_GT(static_cast<double>(imuMsgCount),
+ updateRateImu*now * 0.5);
// Send reset world message
transport::PublisherPtr worldControlPub =
@@ -120,35 +154,47 @@ TEST_F(Sensor_TEST, UpdateAfterReset)
// Count messages again for 2 second
g_hokuyoMsgCount = 0;
+ g_imuMsgCount = 0;
for (i = 0; i < 20; ++i)
{
common::Time::MSleep(100);
}
hokuyoMsgCount = g_hokuyoMsgCount;
+ imuMsgCount = g_imuMsgCount;
now = world->GetSimTime().Double() - now;
- gzdbg << "counted " << hokuyoMsgCount << " messages in "
- << now << " seconds. Expected[" << updateRate * now * 0.5 << "]\n";
+ gzdbg << "counted " << hokuyoMsgCount << " hokuyo messages in "
+ << now << " seconds\n";
+ gzdbg << "counted " << imuMsgCount << " imu messages in "
+ << now << " seconds\n";
// Expect at least 50% of specified update rate
// Note: this is where the failure documented in issue #236 occurs
EXPECT_GT(static_cast<double>(hokuyoMsgCount),
- updateRate*now * 0.5);
+ updateRateHokuyo*now * 0.5);
+ EXPECT_GT(static_cast<double>(imuMsgCount),
+ updateRateImu*now * 0.5);
// Count messages again for 2 more seconds
then = now;
g_hokuyoMsgCount = 0;
+ g_imuMsgCount = 0;
for (i = 0; i < 20; ++i)
{
common::Time::MSleep(100);
}
hokuyoMsgCount = g_hokuyoMsgCount;
+ imuMsgCount = g_imuMsgCount;
now = world->GetSimTime().Double();
- gzdbg << "counted " << hokuyoMsgCount << " messages in "
- << now - then << " seconds\n";
+ gzdbg << "counted " << hokuyoMsgCount << " hokuyo messages in "
+ << now << " seconds\n";
+ gzdbg << "counted " << imuMsgCount << " imu messages in "
+ << now << " seconds\n";
// Expect at least 50% of specified update rate
EXPECT_GT(static_cast<double>(hokuyoMsgCount),
- updateRate*(now-then) * 0.5);
+ updateRateHokuyo*(now-then) * 0.5);
+ EXPECT_GT(static_cast<double>(imuMsgCount),
+ updateRateImu*(now-then) * 0.5);
}
/////////////////////////////////////////////////
diff --git a/gazebo/transport/Node.cc b/gazebo/transport/Node.cc
index 0186340..af28fcd 100644
--- a/gazebo/transport/Node.cc
+++ b/gazebo/transport/Node.cc
@@ -306,7 +306,7 @@ void Node::InsertLatchedMsg(const std::string &_topic, MessagePtr _msg)
std::string Node::GetMsgType(const std::string &_topic) const
{
Callback_M::const_iterator iter = this->callbacks.find(_topic);
- if (iter != this->callbacks.end())
+ if (iter != this->callbacks.end() && !iter->second.empty())
return iter->second.front()->GetMsgType();
return std::string();
diff --git a/test/ServerFixture.cc b/test/ServerFixture.cc
index 773ec8d..9b35c91 100644
--- a/test/ServerFixture.cc
+++ b/test/ServerFixture.cc
@@ -23,7 +23,7 @@
using namespace gazebo;
/////////////////////////////////////////////////
-std::string custom_exec(std::string _cmd)
+std::string gazebo::custom_exec(std::string _cmd)
{
_cmd += " 2>/dev/null";
FILE* pipe = popen(_cmd.c_str(), "r");
@@ -54,6 +54,7 @@ ServerFixture::ServerFixture()
this->gotImage = 0;
this->imgData = NULL;
this->serverThread = NULL;
+ this->uniqueCounter = 0;
gzLogInit("test-", "test.log");
gazebo::common::Console::SetQuiet(false);
@@ -454,6 +455,33 @@ void ServerFixture::GetFrame(const std::string &_cameraName,
}
/////////////////////////////////////////////////
+physics::ModelPtr ServerFixture::SpawnModel(const msgs::Model &_msg)
+{
+ physics::WorldPtr world = physics::get_world();
+ ServerFixture::CheckPointer(world);
+ world->InsertModelString(
+ "<sdf version='" + std::string(SDF_VERSION) + "'>"
+ + msgs::ModelToSDF(_msg)->ToString("")
+ + "</sdf>");
+
+ common::Time wait(10, 0);
+ common::Time wallStart = common::Time::GetWallTime();
+ unsigned int waitCount = 0;
+ while (wait > (common::Time::GetWallTime() - wallStart) &&
+ !this->HasEntity(_msg.name()))
+ {
+ common::Time::MSleep(10);
+ if (++waitCount % 100 == 0)
+ {
+ gzwarn << "Waiting " << waitCount / 100 << " seconds for "
+ << "box to spawn." << std::endl;
+ }
+ }
+
+ return world->GetModel(_msg.name());
+}
+
+/////////////////////////////////////////////////
void ServerFixture::SpawnCamera(const std::string &_modelName,
const std::string &_cameraName,
const math::Vector3 &_pos, const math::Vector3 &_rpy,
@@ -1049,28 +1077,17 @@ void ServerFixture::SpawnCylinder(const std::string &_name,
{
msgs::Factory msg;
std::ostringstream newModelStr;
+ msgs::Model model;
+ model.set_name(_name);
+ model.set_is_static(_static);
+ msgs::Set(model.mutable_pose(), math::Pose(_pos, _rpy));
+ msgs::AddCylinderLink(model, 1.0, 0.5, 1.0);
+ auto link = model.mutable_link(0);
+ link->set_name("body");
+ link->mutable_collision(0)->set_name("geom");
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <collision name ='geom'>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>.5</radius><length>1.0</length>"
- << " </cylinder>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>.5</radius><length>1.0</length>"
- << " </cylinder>"
- << " </geometry>"
- << " </visual>"
- << "</link>"
- << "</model>"
+ << msgs::ModelToSDF(model)->ToString("")
<< "</sdf>";
msg.set_sdf(newModelStr.str());
@@ -1124,27 +1141,19 @@ void ServerFixture::SpawnSphere(const std::string &_name,
{
msgs::Factory msg;
std::ostringstream newModelStr;
+ msgs::Model model;
+ model.set_name(_name);
+ model.set_is_static(_static);
+ msgs::Set(model.mutable_pose(), math::Pose(_pos, _rpy));
+ msgs::AddSphereLink(model, 1.0, _radius);
+ auto link = model.mutable_link(0);
+ link->set_name("body");
+ link->mutable_collision(0)->set_name("geom");
+ msgs::Set(link->mutable_inertial()->mutable_pose(),
+ math::Pose(_cog, math::Quaternion()));
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <inertial>"
- << " <pose>" << _cog << " 0 0 0</pose>"
- << " </inertial>"
- << " <collision name ='geom'>"
- << " <geometry>"
- << " <sphere><radius>" << _radius << "</radius></sphere>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <sphere><radius>" << _radius << "</radius></sphere>"
- << " </geometry>"
- << " </visual>"
- << "</link>"
- << "</model>"
+ << msgs::ModelToSDF(model)->ToString("")
<< "</sdf>";
msg.set_sdf(newModelStr.str());
@@ -1162,24 +1171,17 @@ void ServerFixture::SpawnBox(const std::string &_name,
{
msgs::Factory msg;
std::ostringstream newModelStr;
+ msgs::Model model;
+ model.set_name(_name);
+ model.set_is_static(_static);
+ msgs::Set(model.mutable_pose(), math::Pose(_pos, _rpy));
+ msgs::AddBoxLink(model, 1.0, _size);
+ auto link = model.mutable_link(0);
+ link->set_name("body");
+ link->mutable_collision(0)->set_name("geom");
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <collision name ='geom'>"
- << " <geometry>"
- << " <box><size>" << _size << "</size></box>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <box><size>" << _size << "</size></box>"
- << " </geometry>"
- << " </visual>"
- << "</link>"
- << "</model>"
+ << msgs::ModelToSDF(model)->ToString("")
<< "</sdf>";
msg.set_sdf(newModelStr.str());
@@ -1236,14 +1238,15 @@ void ServerFixture::SpawnEmptyLink(const std::string &_name,
{
msgs::Factory msg;
std::ostringstream newModelStr;
+ msgs::Model model;
+ model.set_name(_name);
+ model.set_is_static(_static);
+ msgs::Set(model.mutable_pose(), math::Pose(_pos, _rpy));
+ model.add_link();
+ model.mutable_link(0)->set_name("body");
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << "</link>"
- << "</model>"
+ << msgs::ModelToSDF(model)->ToString("")
<< "</sdf>";
msg.set_sdf(newModelStr.str());
@@ -1359,3 +1362,11 @@ void ServerFixture::GetMemInfo(double &_resident, double &_share)
return;
#endif
}
+
+/////////////////////////////////////////////////
+std::string ServerFixture::GetUniqueString(const std::string &_prefix)
+{
+ std::ostringstream stream;
+ stream << _prefix << this->uniqueCounter++;
+ return stream.str();
+}
diff --git a/test/ServerFixture.hh b/test/ServerFixture.hh
index 5b0c1ab..ce5d1ed 100644
--- a/test/ServerFixture.hh
+++ b/test/ServerFixture.hh
@@ -52,520 +52,549 @@
#include "test_config.h"
-using namespace gazebo;
-
-std::string custom_exec(std::string _cmd);
-
-class ServerFixture : public testing::Test
+namespace gazebo
{
- /// \brief Constructor
- protected: ServerFixture();
-
- /// \brief Tear down the test fixture. This gets called by gtest.
- protected: virtual void TearDown();
-
- /// \brief Unload the test fixture.
- protected: virtual void Unload();
-
- /// \brief Load a world based on a filename.
- /// \param[in] _worldFilename Name of the world to load.
- protected: virtual void Load(const std::string &_worldFilename);
-
- /// \brief Load a world based on a filename and set simulation
- /// paused/un-paused.
- /// \param[in] _worldFilename Name of the world to load.
- /// \param[in] _paused True to start the world paused.
- protected: virtual void Load(const std::string &_worldFilename, bool _paused);
-
- /// \brief Load a world based on a filename and set simulation
- /// paused/un-paused, and specify physics engine.
- /// \param[in] _worldFilename Name of the world to load.
- /// \param[in] _paused True to start the world paused.
- /// \param[in] _physics Name of the physics engine.
- protected: virtual void Load(const std::string &_worldFilename,
- bool _paused, const std::string &_physics);
-
- /// \brief Run the server.
- /// \param[in] _worldFilename Name of the world to run in simulation.
- protected: void RunServer(const std::string &_worldFilename);
-
- /// \brief Get a pointer to the rendering scene.
- /// \param[in] _sceneName Name of the scene to get.
- protected: rendering::ScenePtr GetScene(
- const std::string &_sceneName = "default");
-
- /// \brief Run the server, start paused/unpaused, and specify the physics
- /// engine.
- /// \param[in] _worldFilename Name of the world to load.
- /// \param[in] _paused True to start the world paused.
- /// \param[in] _physics Name of the physics engine.
- protected: void RunServer(const std::string &_worldFilename, bool _paused,
- const std::string &_physics);
-
- /// \brief Function that received world stastics messages.
- /// \param[in] _msg World statistics message.
- protected: void OnStats(ConstWorldStatisticsPtr &_msg);
-
- /// \brief Set a running simulation paused/unpaused.
- protected: void SetPause(bool _pause);
-
- /// \brief Get the real-time factor.
- /// \return The percent real time simulation is running.
- protected: double GetPercentRealTime() const;
-
- /// \brief Function that received poses messages from a running
- /// simulation.
- /// \param[in] _msg Pose message.
- protected: void OnPose(ConstPosesStampedPtr &_msg);
-
- /// \brief Get the pose of an entity.
- /// \param[in] _name Name of the entity.
- /// \return Pose of the named entity.
- protected: math::Pose GetEntityPose(const std::string &_name);
-
- /// \brief Return true if the named entity exists.
- /// \param[in] _name Name of the entity to check for.
- /// \return True if the entity exists.
- protected: bool HasEntity(const std::string &_name);
-
- /// \brief Print image data to screen. This is used to generate test data.
- /// \param[in] _name Name to associate with the printed data.
- /// \param[in] _image The raw image data.
- /// \param[in] _width Width of the image.
- /// \param[in] _height Height of the image.
- /// \param[in] _depth Pixel depth.
- protected: void PrintImage(const std::string &_name, unsigned char **_image,
- unsigned int _width, unsigned int _height, unsigned int _depth);
-
- /// \brief Print laser scan to screen. This is used to generate test data.
- /// \param[in] _name Name to associate with the printed data.
- /// \param[in] _scan The laser scan data.
- /// \param[in] _sampleCount Number of samples in the scan data.
- protected: void PrintScan(const std::string &_name, double *_scan,
- unsigned int _sampleCount);
-
- /// \brief Function to compare two float arrays (for example two laser
- /// scans).
- /// \param[in] _scanA First float array.
- /// \param[in] _scanB Second float array.
- /// \param[in] _sampleCount Number of samples in each float array.
- /// \param[out] _diffMax Maximum difference between the two arrays.
- /// \param[out] _diffSum Sum of the differences between the two arrays.
- /// \param[out] _diffAvg Average difference between the two arrays.
- protected: void FloatCompare(float *_scanA, float *_scanB,
- unsigned int _sampleCount, float &_diffMax,
- float &_diffSum, float &_diffAvg);
-
- /// \brief Function to compare two double arrays (for example two laser
- /// scans).
- /// \param[in] _scanA First double array.
- /// \param[in] _scanB Second double array.
- /// \param[in] _sampleCount Number of samples in each double array.
- /// \param[out] _diffMax Maximum difference between the two arrays.
- /// \param[out] _diffSum Sum of the differences between the two arrays.
- /// \param[out] _diffAvg Average difference between the two arrays.
- protected: void DoubleCompare(double *_scanA, double *_scanB,
- unsigned int _sampleCount, double &_diffMax,
- double &_diffSum, double &_diffAvg);
-
- /// \brief Function to compare two images.
- /// \param[in] _imageA First image to compare.
- /// \param[in] _imageB Second image to compare.
- /// \param[in] _width Width of both images.
- /// \param[in] _height Height of both images.
- /// \param[in] _depth Depth of both images.
- /// \param[out] _diffMax Maximum difference between the two arrays.
- /// \param[out] _diffSum Sum of the differences between the two arrays.
- /// \param[out] _diffAvg Average difference between the two arrays.
- protected: void ImageCompare(unsigned char *_imageA,
- unsigned char *_imageB,
- unsigned int _width, unsigned int _height, unsigned int _depth,
- unsigned int &_diffMax, unsigned int &_diffSum,
- double &_diffAvg);
-
- /// \brief Function that receives new image frames.
- /// \param[in] _image Image data.
- /// \param[in] _width Width of the image frame.
- /// \param[in] _height Height of the image frame.
- /// \param[in] _depth Depth of the image frame.
- /// \param[in] _format Pixel format.
- private: void OnNewFrame(const unsigned char *_image,
- unsigned int _width, unsigned int _height,
- unsigned int _depth,
- const std::string &/*_format*/);
-
- /// \brief Get an image frame from a camera.
- /// \param[in] _cameraName Name of the camera to get a frame from.
- /// \param[out] _imgData Array that receives the image data.
- /// \param[in] _width Width of the image frame.
- /// \param[in] _height Height of the image frame.
- protected: void GetFrame(const std::string &_cameraName,
- unsigned char **_imgData, unsigned int &_width,
- unsigned int &_height);
-
- /// \brief Spawn a camera.
- /// \param[in] _modelName Name of the model.
- /// \param[in] _cameraName Name of the camera.
- /// \param[in] _pos Camera position.
- /// \param[in] _rpy Camera roll, pitch, yaw.
- /// \param[in] _width Output image width.
- /// \param[in] _height Output image height.
- /// \param[in] _rate Output Hz.
- /// \param[in] _noiseType Type of noise to apply.
- /// \param[in] _noiseMean Mean noise value.
- /// \param[in] _noiseStdDev Standard deviation of the noise.
- /// \param[in] _distortionK1 Distortion coefficient k1.
- /// \param[in] _distortionK2 Distortion coefficient k2.
- /// \param[in] _distortionK3 Distortion coefficient k3.
- /// \param[in] _distortionP1 Distortion coefficient P1.
- /// \param[in] _distortionP2 Distortion coefficient p2.
- /// \param[in] _cx Normalized optical center x, used for distortion.
- /// \param[in] _cy Normalized optical center y, used for distortion.
- protected: void SpawnCamera(const std::string &_modelName,
- const std::string &_cameraName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- unsigned int _width = 320, unsigned int _height = 240,
- double _rate = 25,
- const std::string &_noiseType = "",
- double _noiseMean = 0.0, double _noiseStdDev = 0.0,
- bool _distortion = false, double _distortionK1 = 0.0,
- double _distortionK2 = 0.0, double _distortionK3 = 0.0,
- double _distortionP1 = 0.0, double _distortionP2 = 0.0,
- double _cx = 0.5, double _cy = 0.5);
-
- /// \brief Spawn a laser.
- /// \param[in] _modelName Name of the model.
- /// \param[in] _raySensorName Name of the laser.
- /// \param[in] _pos Camera position.
- /// \param[in] _rpy Camera roll, pitch, yaw.
- /// \param[in] _hMinAngle Horizontal min angle
- /// \param[in] _hMaxAngle Horizontal max angle
- /// \param[in] _minRange Min range
- /// \param[in] _maxRange Max range
- /// \param[in] _rangeResolution Resolution of the scan
- /// \param[in] _samples Number of samples.
- /// \param[in] _rate Output Hz.
- /// \param[in] _noiseType Type of noise to apply.
- /// \param[in] _noiseMean Mean noise value.
- /// \param[in] _noiseStdDev Standard deviation of the noise.
- protected: void SpawnRaySensor(const std::string &_modelName,
- const std::string &_raySensorName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
- double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
- double _minRange = 0.08, double _maxRange = 10,
- double _rangeResolution = 0.01, unsigned int _samples = 640,
- unsigned int _vSamples = 1, double _hResolution = 1.0,
- double _vResolution = 1.0,
- const std::string &_noiseType = "", double _noiseMean = 0.0,
- double _noiseStdDev = 0.0);
-
- /// \brief Spawn a gpu laser.
- /// \param[in] _modelName Name of the model.
- /// \param[in] _raySensorName Name of the laser.
- /// \param[in] _pos Camera position.
- /// \param[in] _rpy Camera roll, pitch, yaw.
- /// \param[in] _hMinAngle Horizontal min angle
- /// \param[in] _hMaxAngle Horizontal max angle
- /// \param[in] _minRange Min range
- /// \param[in] _maxRange Max range
- /// \param[in] _rangeResolution Resolution of the scan
- /// \param[in] _samples Number of samples.
- /// \param[in] _rate Output Hz.
- /// \param[in] _noiseType Type of noise to apply.
- /// \param[in] _noiseMean Mean noise value.
- /// \param[in] _noiseStdDev Standard deviation of the noise.
- protected: void SpawnGpuRaySensor(const std::string &_modelName,
- const std::string &_raySensorName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
- double _minRange = 0.08, double _maxRange = 10,
- double _rangeResolution = 0.01, unsigned int _samples = 640,
- const std::string &_noiseType = "", double _noiseMean = 0.0,
- double _noiseStdDev = 0.0);
-
- /// \brief Spawn an imu sensor laser.
- /// \param[in] _modelName Name of the model.
- /// \param[in] _imuSensorName Name of the imu sensor.
- /// \param[in] _pos Camera position.
- /// \param[in] _rpy Camera roll, pitch, yaw.
- /// \param[in] _noiseType Type of noise to apply.
- /// \param[in] _noiseMean Mean noise value.
- /// \param[in] _noiseStdDev Standard deviation of the noise.
- /// \param[in] _accelNoiseMean Acceleration based noise mean.
- /// \param[in] _accelNoiseStdDev Acceleration based noise standard
- /// deviation.
- /// \param[in] _accelBiasMean Acceleration mean bias
- /// \param[in] _accelBiasStdDev Acceleration standard deviation bias
- protected: void SpawnImuSensor(const std::string &_modelName,
- const std::string &_imuSensorName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- const std::string &_noiseType = "",
- double _rateNoiseMean = 0.0, double _rateNoiseStdDev = 0.0,
- double _rateBiasMean = 0.0, double _rateBiasStdDev = 0.0,
- double _accelNoiseMean = 0.0, double _accelNoiseStdDev = 0.0,
- double _accelBiasMean = 0.0, double _accelBiasStdDev = 0.0);
-
- /// \brief Spawn a contact sensor with the specified collision geometry
- /// \param[in] _name Model name
- /// \param[in] _sensorName Sensor name
- /// \param[in] _collisionType Type of collision, box or cylinder
- /// \param[in] _pos World position
- /// \param[in] _rpy World rotation in Euler angles
- /// \param[in] _static True to make the model static
- protected: void SpawnUnitContactSensor(const std::string &_name,
- const std::string &_sensorName,
- const std::string &_collisionType, const math::Vector3 &_pos,
- const math::Vector3 &_rpy, bool _static = false);
-
- /// \brief Spawn an IMU sensor on a link
- /// \param[in] _name Model name
- /// \param[in] _sensorName Sensor name
- /// \param[in] _collisionType Type of collision, box or cylinder
- /// \param[in] _topic Topic to publish IMU data to
- /// \param[in] _pos World position
- /// \param[in] _rpy World rotation in Euler angles
- /// \param[in] _static True to make the model static
- protected: void SpawnUnitImuSensor(const std::string &_name,
- const std::string &_sensorName,
- const std::string &_collisionType,
- const std::string &_topic, const math::Vector3 &_pos,
- const math::Vector3 &_rpy, bool _static = false);
-
- /// \brief generate a gtest failure from a timeout error and display a
- /// log message about the problem.
- /// \param[in] log_msg: error msg related to the timeout
- /// \param[in] timeoutCS: failing period (in centiseconds)
- private: void launchTimeoutFailure(const char *_logMsg, const int _timeoutCS);
-
- /// \brief Spawn an Wireless transmitter sensor on a link
- /// \param[in] _name Model name
- /// \param[in] _sensorName Sensor name
- /// \param[in] _pos World position
- /// \param[in] _rpy World rotation in Euler angles
- /// \param[in] _essid Service set identifier (network name)
- /// \param[in] _freq Frequency of transmission (MHz)
- /// \param[in] _power Transmission power (dBm)
- /// \param[in] _gain Antenna gain (dBi)
- /// \param[in] _visualize Enable sensor visualization
- protected: void SpawnWirelessTransmitterSensor(const std::string &_name,
- const std::string &_sensorName,
- const math::Vector3 &_pos,
- const math::Vector3 &_rpy,
- const std::string &_essid,
- double _freq,
- double _power,
- double _gain,
- bool _visualize = true);
-
- /// \brief Spawn an Wireless receiver sensor on a link
- /// \param[in] _name Model name
- /// \param[in] _sensorName Sensor name
- /// \param[in] _pos World position
- /// \param[in] _rpy World rotation in Euler angles
- /// \param[in] _minFreq Minimum frequency to be filtered (MHz)
- /// \param[in] _maxFreq Maximum frequency to be filtered (MHz)
- /// \param[in] _power Transmission power (dBm)
- /// \param[in] _gain Antenna gain (dBi)
- /// \param[in] _sensitivity Receiver sensitibity (dBm)
- /// \param[in] _visualize Enable sensor visualization
- protected: void SpawnWirelessReceiverSensor(const std::string &_name,
- const std::string &_sensorName,
- const math::Vector3 &_pos,
- const math::Vector3 &_rpy,
- double _minFreq,
- double _maxFreq,
- double _power,
- double _gain,
- double _sensitivity,
- bool _visualize = true);
-
- /// \brief Wait for a number of ms. and attempts until the entity is spawned
- /// \param[in] _name Model name
- /// \param[in] _sleepEach Number of milliseconds to sleep in each iteration
- /// \param[in] _retries Number of iterations until give up
- protected: void WaitUntilEntitySpawn(const std::string &_name,
- unsigned int _sleepEach,
- int _retries);
-
- /// \brief Wait for a number of ms. and attempts until the sensor is spawned
- /// \param[in] _name Sensor name
- /// \param[in] _sleepEach Number of milliseconds to sleep in each iteration
- /// \param[in] _retries Number of iterations until give up
- protected: void WaitUntilSensorSpawn(const std::string &_name,
- unsigned int _sleepEach,
- int _retries);
-
- /// \brief Spawn a light.
- /// \param[in] _name Name for the light.
- /// \param[in] _size Type of light - "spot", "directional", or "point".
- /// \param[in] _pos Position for the light.
- /// \param[in] _rpy Roll, pitch, yaw for the light.
- /// \param[in] _diffuse Diffuse color of the light.
- /// \param[in] _specular Specular color of the light.
- /// \param[in] _direction Direction of the light ("spot" and "directional").
- /// \param[in] _attenuationRange Range of attenuation.
- /// \param[in] _attenuationConstant Constant component of attenuation
- /// \param[in] _attenuationLinear Linear component of attenuation
- /// \param[in] _attenuationQuadratic Quadratic component of attenuation
- /// \param[in] _spotInnerAngle Inner angle ("spot" only).
- /// \param[in] _spotOuterAngle Outer angle ("spot" only).
- /// \param[in] _spotFallOff Fall off ("spot" only).
- /// \param[in] _castShadows True to cast shadows.
- protected: void SpawnLight(const std::string &_name, const std::string &_type,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- const common::Color &_diffuse = common::Color::White,
- const common::Color &_specular = common::Color::White,
- const math::Vector3 &_direction = -math::Vector3::UnitZ,
- double _attenuationRange = 20,
- double _attenuationConstant = 0.5,
- double _attenuationLinear = 0.01,
- double _attenuationQuadratic = 0.001,
- double _spotInnerAngle = 0,
- double _spotOuterAngle = 0,
- double _spotFallOff = 0,
- bool _castShadows = true);
-
- /// \brief Spawn a cylinder
- /// \param[in] _name Name for the model.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- protected: void SpawnCylinder(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _static = false);
-
- /// \brief Spawn a sphere
- /// \param[in] _name Name for the model.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- /// \param[in] _wait True to wait for the sphere to spawn before
- /// returning.
- protected: void SpawnSphere(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _wait = true, bool _static = false);
-
- /// \brief Spawn a sphere
- /// \param[in] _name Name for the model.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _cog Center of gravity.
- /// \param[in] _radius Sphere radius.
- /// \param[in] _static True to make the model static.
- /// \param[in] _wait True to wait for the sphere to spawn before
- /// returning.
- protected: void SpawnSphere(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- const math::Vector3 &_cog, double _radius,
- bool _wait = true, bool _static = false);
-
- /// \brief Spawn a box.
- /// \param[in] _name Name for the model.
- /// \param[in] _size Size of the box.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- protected: void SpawnBox(const std::string &_name,
- const math::Vector3 &_size, const math::Vector3 &_pos,
- const math::Vector3 &_rpy, bool _static = false);
-
- /// \brief Spawn a triangle mesh.
- /// \param[in] _name Name for the model.
- /// \param[in] _modelPath Path to the mesh file.
- /// \param[in] _scale Scaling factor.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- protected: void SpawnTrimesh(const std::string &_name,
- const std::string &_modelPath, const math::Vector3 &_scale,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _static = false);
-
- /// \brief Spawn an empty link.
- /// \param[in] _name Name for the model.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- protected: void SpawnEmptyLink(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _static = false);
-
- /// \brief Spawn a model from file.
- /// \param[in] _filename File to load a model from.
- protected: void SpawnModel(const std::string &_filename);
-
- /// \brief Send a factory message based on an SDF string.
- /// \param[in] _sdf SDF string to publish.
- protected: void SpawnSDF(const std::string &_sdf);
-
- /// \brief Load a plugin.
- /// \param[in] _filename Plugin filename to load.
- /// \param[in] _name Name to associate with with the plugin.
- protected: void LoadPlugin(const std::string &_filename,
- const std::string &_name);
-
- /// \brief Get a pointer to a model.
- /// \param[in] _name Name of the model to get.
- /// \return Pointer to the model, or NULL if the model was not found.
- protected: physics::ModelPtr GetModel(const std::string &_name);
-
- /// \brief Remove a model by name.
- /// \param[in] _name Name of the model to remove.
- protected: void RemoveModel(const std::string &_name);
-
- /// \brief Remove a plugin.
- /// \param[in] _name Name of the plugin to remove.
- protected: void RemovePlugin(const std::string &_name);
-
- /// \brief Get the current memory information.
- /// \param[out] _resident Resident memory.
- /// \param[out] _share Shared memory.
- protected: void GetMemInfo(double &_resident, double &_share);
-
- /// \brief Pointer the Gazebo server.
- protected: Server *server;
-
- /// \brief Pointer the thread the runs the server.
- protected: boost::thread *serverThread;
-
- /// \brief Pointer to a node for communication.
- protected: transport::NodePtr node;
-
- /// \brief Pose subscription.
- protected: transport::SubscriberPtr poseSub;
-
- /// \brief World statistics subscription.
- protected: transport::SubscriberPtr statsSub;
-
- /// \brief Factory publisher.
- protected: transport::PublisherPtr factoryPub;
-
- /// \brief Request publisher.
- protected: transport::PublisherPtr requestPub;
-
- /// \brief Map of received poses.
- protected: std::map<std::string, math::Pose> poses;
-
- /// \brief Mutex to protect data structures that store messages.
- protected: boost::mutex receiveMutex;
-
- /// \brief Image data
- private: unsigned char **imgData;
-
- /// \brief Increments when images are received.
- private: int gotImage;
-
- /// \brief Current simulation time, real time, and pause time.
- protected: common::Time simTime, realTime, pauseTime;
-
- /// \brief Current percent realtime.
- private: double percentRealTime;
-
- /// \brief True if paused.
- private: bool paused;
-
- /// \brief True if server is running.
- private: bool serverRunning;
-};
+ std::string custom_exec(std::string _cmd);
+
+ class ServerFixture : public testing::Test
+ {
+ /// \brief Constructor
+ protected: ServerFixture();
+
+ /// \brief Tear down the test fixture. This gets called by gtest.
+ protected: virtual void TearDown();
+
+ /// \brief Unload the test fixture.
+ protected: virtual void Unload();
+
+ /// \brief Load a world based on a filename.
+ /// \param[in] _worldFilename Name of the world to load.
+ protected: virtual void Load(const std::string &_worldFilename);
+
+ /// \brief Load a world based on a filename and set simulation
+ /// paused/un-paused.
+ /// \param[in] _worldFilename Name of the world to load.
+ /// \param[in] _paused True to start the world paused.
+ protected: virtual void Load(const std::string &_worldFilename,
+ bool _paused);
+
+ /// \brief Load a world based on a filename and set simulation
+ /// paused/un-paused, and specify physics engine.
+ /// \param[in] _worldFilename Name of the world to load.
+ /// \param[in] _paused True to start the world paused.
+ /// \param[in] _physics Name of the physics engine.
+ protected: virtual void Load(const std::string &_worldFilename,
+ bool _paused, const std::string &_physics);
+
+ /// \brief Run the server.
+ /// \param[in] _worldFilename Name of the world to run in simulation.
+ protected: void RunServer(const std::string &_worldFilename);
+
+ /// \brief Get a pointer to the rendering scene.
+ /// \param[in] _sceneName Name of the scene to get.
+ protected: rendering::ScenePtr GetScene(
+ const std::string &_sceneName = "default");
+
+ /// \brief Run the server, start paused/unpaused, and specify the physics
+ /// engine.
+ /// \param[in] _worldFilename Name of the world to load.
+ /// \param[in] _paused True to start the world paused.
+ /// \param[in] _physics Name of the physics engine.
+ protected: void RunServer(const std::string &_worldFilename, bool _paused,
+ const std::string &_physics);
+
+ /// \brief Function that received world stastics messages.
+ /// \param[in] _msg World statistics message.
+ protected: void OnStats(ConstWorldStatisticsPtr &_msg);
+
+ /// \brief Set a running simulation paused/unpaused.
+ protected: void SetPause(bool _pause);
+
+ /// \brief Get the real-time factor.
+ /// \return The percent real time simulation is running.
+ protected: double GetPercentRealTime() const;
+
+ /// \brief Function that received poses messages from a running
+ /// simulation.
+ /// \param[in] _msg Pose message.
+ protected: void OnPose(ConstPosesStampedPtr &_msg);
+
+ /// \brief Get the pose of an entity.
+ /// \param[in] _name Name of the entity.
+ /// \return Pose of the named entity.
+ protected: math::Pose GetEntityPose(const std::string &_name);
+
+ /// \brief Return true if the named entity exists.
+ /// \param[in] _name Name of the entity to check for.
+ /// \return True if the entity exists.
+ protected: bool HasEntity(const std::string &_name);
+
+ /// \brief Print image data to screen. This is used to generate test data.
+ /// \param[in] _name Name to associate with the printed data.
+ /// \param[in] _image The raw image data.
+ /// \param[in] _width Width of the image.
+ /// \param[in] _height Height of the image.
+ /// \param[in] _depth Pixel depth.
+ protected: void PrintImage(const std::string &_name, unsigned char **_image,
+ unsigned int _width, unsigned int _height,
+ unsigned int _depth);
+
+ /// \brief Print laser scan to screen. This is used to generate test data.
+ /// \param[in] _name Name to associate with the printed data.
+ /// \param[in] _scan The laser scan data.
+ /// \param[in] _sampleCount Number of samples in the scan data.
+ protected: void PrintScan(const std::string &_name, double *_scan,
+ unsigned int _sampleCount);
+
+ /// \brief Function to compare two float arrays (for example two laser
+ /// scans).
+ /// \param[in] _scanA First float array.
+ /// \param[in] _scanB Second float array.
+ /// \param[in] _sampleCount Number of samples in each float array.
+ /// \param[out] _diffMax Maximum difference between the two arrays.
+ /// \param[out] _diffSum Sum of the differences between the two arrays.
+ /// \param[out] _diffAvg Average difference between the two arrays.
+ protected: void FloatCompare(float *_scanA, float *_scanB,
+ unsigned int _sampleCount, float &_diffMax,
+ float &_diffSum, float &_diffAvg);
+
+ /// \brief Function to compare two double arrays (for example two laser
+ /// scans).
+ /// \param[in] _scanA First double array.
+ /// \param[in] _scanB Second double array.
+ /// \param[in] _sampleCount Number of samples in each double array.
+ /// \param[out] _diffMax Maximum difference between the two arrays.
+ /// \param[out] _diffSum Sum of the differences between the two arrays.
+ /// \param[out] _diffAvg Average difference between the two arrays.
+ protected: void DoubleCompare(double *_scanA, double *_scanB,
+ unsigned int _sampleCount, double &_diffMax,
+ double &_diffSum, double &_diffAvg);
+
+ /// \brief Function to compare two images.
+ /// \param[in] _imageA First image to compare.
+ /// \param[in] _imageB Second image to compare.
+ /// \param[in] _width Width of both images.
+ /// \param[in] _height Height of both images.
+ /// \param[in] _depth Depth of both images.
+ /// \param[out] _diffMax Maximum difference between the two arrays.
+ /// \param[out] _diffSum Sum of the differences between the two arrays.
+ /// \param[out] _diffAvg Average difference between the two arrays.
+ protected: void ImageCompare(unsigned char *_imageA,
+ unsigned char *_imageB,
+ unsigned int _width, unsigned int _height,
+ unsigned int _depth,
+ unsigned int &_diffMax, unsigned int &_diffSum,
+ double &_diffAvg);
+
+ /// \brief Function that receives new image frames.
+ /// \param[in] _image Image data.
+ /// \param[in] _width Width of the image frame.
+ /// \param[in] _height Height of the image frame.
+ /// \param[in] _depth Depth of the image frame.
+ /// \param[in] _format Pixel format.
+ private: void OnNewFrame(const unsigned char *_image,
+ unsigned int _width, unsigned int _height,
+ unsigned int _depth,
+ const std::string &/*_format*/);
+
+ /// \brief Get an image frame from a camera.
+ /// \param[in] _cameraName Name of the camera to get a frame from.
+ /// \param[out] _imgData Array that receives the image data.
+ /// \param[in] _width Width of the image frame.
+ /// \param[in] _height Height of the image frame.
+ protected: void GetFrame(const std::string &_cameraName,
+ unsigned char **_imgData, unsigned int &_width,
+ unsigned int &_height);
+
+ /// \brief Spawn a model from a msgs::Model and return ModelPtr.
+ /// \param[in] _msg Model message.
+ /// \return Pointer to model.
+ protected: physics::ModelPtr SpawnModel(const msgs::Model &_msg);
+
+ /// \brief Check that a pointer is not NULL. A function is created
+ /// for this purpose, since ASSERT's cannot be called from non-void
+ /// functions.
+ /// \param[in] _ptr Pointer to verify is not NULL.
+ protected: template<typename T>
+ static void CheckPointer(boost::shared_ptr<T> _ptr)
+ {
+ ASSERT_TRUE(_ptr != NULL);
+ }
+
+ /// \brief Spawn a camera.
+ /// \param[in] _modelName Name of the model.
+ /// \param[in] _cameraName Name of the camera.
+ /// \param[in] _pos Camera position.
+ /// \param[in] _rpy Camera roll, pitch, yaw.
+ /// \param[in] _width Output image width.
+ /// \param[in] _height Output image height.
+ /// \param[in] _rate Output Hz.
+ /// \param[in] _noiseType Type of noise to apply.
+ /// \param[in] _noiseMean Mean noise value.
+ /// \param[in] _noiseStdDev Standard deviation of the noise.
+ /// \param[in] _distortionK1 Distortion coefficient k1.
+ /// \param[in] _distortionK2 Distortion coefficient k2.
+ /// \param[in] _distortionK3 Distortion coefficient k3.
+ /// \param[in] _distortionP1 Distortion coefficient P1.
+ /// \param[in] _distortionP2 Distortion coefficient p2.
+ /// \param[in] _cx Normalized optical center x, used for distortion.
+ /// \param[in] _cy Normalized optical center y, used for distortion.
+ protected: void SpawnCamera(const std::string &_modelName,
+ const std::string &_cameraName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ unsigned int _width = 320, unsigned int _height = 240,
+ double _rate = 25,
+ const std::string &_noiseType = "",
+ double _noiseMean = 0.0, double _noiseStdDev = 0.0,
+ bool _distortion = false, double _distortionK1 = 0.0,
+ double _distortionK2 = 0.0, double _distortionK3 = 0.0,
+ double _distortionP1 = 0.0, double _distortionP2 = 0.0,
+ double _cx = 0.5, double _cy = 0.5);
+
+ /// \brief Spawn a laser.
+ /// \param[in] _modelName Name of the model.
+ /// \param[in] _raySensorName Name of the laser.
+ /// \param[in] _pos Camera position.
+ /// \param[in] _rpy Camera roll, pitch, yaw.
+ /// \param[in] _hMinAngle Horizontal min angle
+ /// \param[in] _hMaxAngle Horizontal max angle
+ /// \param[in] _minRange Min range
+ /// \param[in] _maxRange Max range
+ /// \param[in] _rangeResolution Resolution of the scan
+ /// \param[in] _samples Number of samples.
+ /// \param[in] _rate Output Hz.
+ /// \param[in] _noiseType Type of noise to apply.
+ /// \param[in] _noiseMean Mean noise value.
+ /// \param[in] _noiseStdDev Standard deviation of the noise.
+ protected: void SpawnRaySensor(const std::string &_modelName,
+ const std::string &_raySensorName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
+ double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
+ double _minRange = 0.08, double _maxRange = 10,
+ double _rangeResolution = 0.01, unsigned int _samples = 640,
+ unsigned int _vSamples = 1, double _hResolution = 1.0,
+ double _vResolution = 1.0,
+ const std::string &_noiseType = "", double _noiseMean = 0.0,
+ double _noiseStdDev = 0.0);
+
+ /// \brief Spawn a gpu laser.
+ /// \param[in] _modelName Name of the model.
+ /// \param[in] _raySensorName Name of the laser.
+ /// \param[in] _pos Camera position.
+ /// \param[in] _rpy Camera roll, pitch, yaw.
+ /// \param[in] _hMinAngle Horizontal min angle
+ /// \param[in] _hMaxAngle Horizontal max angle
+ /// \param[in] _minRange Min range
+ /// \param[in] _maxRange Max range
+ /// \param[in] _rangeResolution Resolution of the scan
+ /// \param[in] _samples Number of samples.
+ /// \param[in] _rate Output Hz.
+ /// \param[in] _noiseType Type of noise to apply.
+ /// \param[in] _noiseMean Mean noise value.
+ /// \param[in] _noiseStdDev Standard deviation of the noise.
+ protected: void SpawnGpuRaySensor(const std::string &_modelName,
+ const std::string &_raySensorName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
+ double _minRange = 0.08, double _maxRange = 10,
+ double _rangeResolution = 0.01, unsigned int _samples = 640,
+ const std::string &_noiseType = "", double _noiseMean = 0.0,
+ double _noiseStdDev = 0.0);
+
+ /// \brief Spawn an imu sensor laser.
+ /// \param[in] _modelName Name of the model.
+ /// \param[in] _imuSensorName Name of the imu sensor.
+ /// \param[in] _pos Camera position.
+ /// \param[in] _rpy Camera roll, pitch, yaw.
+ /// \param[in] _noiseType Type of noise to apply.
+ /// \param[in] _noiseMean Mean noise value.
+ /// \param[in] _noiseStdDev Standard deviation of the noise.
+ /// \param[in] _accelNoiseMean Acceleration based noise mean.
+ /// \param[in] _accelNoiseStdDev Acceleration based noise standard
+ /// deviation.
+ /// \param[in] _accelBiasMean Acceleration mean bias
+ /// \param[in] _accelBiasStdDev Acceleration standard deviation bias
+ protected: void SpawnImuSensor(const std::string &_modelName,
+ const std::string &_imuSensorName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ const std::string &_noiseType = "",
+ double _rateNoiseMean = 0.0, double _rateNoiseStdDev = 0.0,
+ double _rateBiasMean = 0.0, double _rateBiasStdDev = 0.0,
+ double _accelNoiseMean = 0.0, double _accelNoiseStdDev = 0.0,
+ double _accelBiasMean = 0.0, double _accelBiasStdDev = 0.0);
+
+ /// \brief Spawn a contact sensor with the specified collision geometry
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _collisionType Type of collision, box or cylinder
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _static True to make the model static
+ protected: void SpawnUnitContactSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType, const math::Vector3 &_pos,
+ const math::Vector3 &_rpy, bool _static = false);
+
+ /// \brief Spawn an IMU sensor on a link
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _collisionType Type of collision, box or cylinder
+ /// \param[in] _topic Topic to publish IMU data to
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _static True to make the model static
+ protected: void SpawnUnitImuSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType,
+ const std::string &_topic, const math::Vector3 &_pos,
+ const math::Vector3 &_rpy, bool _static = false);
+
+ /// \brief generate a gtest failure from a timeout error and display a
+ /// log message about the problem.
+ /// \param[in] log_msg: error msg related to the timeout
+ /// \param[in] timeoutCS: failing period (in centiseconds)
+ private: void launchTimeoutFailure(const char *_logMsg,
+ const int _timeoutCS);
+
+ /// \brief Spawn an Wireless transmitter sensor on a link
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _essid Service set identifier (network name)
+ /// \param[in] _freq Frequency of transmission (MHz)
+ /// \param[in] _power Transmission power (dBm)
+ /// \param[in] _gain Antenna gain (dBi)
+ /// \param[in] _visualize Enable sensor visualization
+ protected: void SpawnWirelessTransmitterSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const math::Vector3 &_pos,
+ const math::Vector3 &_rpy,
+ const std::string &_essid,
+ double _freq,
+ double _power,
+ double _gain,
+ bool _visualize = true);
+
+ /// \brief Spawn an Wireless receiver sensor on a link
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _minFreq Minimum frequency to be filtered (MHz)
+ /// \param[in] _maxFreq Maximum frequency to be filtered (MHz)
+ /// \param[in] _power Transmission power (dBm)
+ /// \param[in] _gain Antenna gain (dBi)
+ /// \param[in] _sensitivity Receiver sensitibity (dBm)
+ /// \param[in] _visualize Enable sensor visualization
+ protected: void SpawnWirelessReceiverSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const math::Vector3 &_pos,
+ const math::Vector3 &_rpy,
+ double _minFreq,
+ double _maxFreq,
+ double _power,
+ double _gain,
+ double _sensitivity,
+ bool _visualize = true);
+
+ /// \brief Wait for a number of ms. and attempts until the entity is spawned
+ /// \param[in] _name Model name
+ /// \param[in] _sleepEach Number of milliseconds to sleep in each iteration
+ /// \param[in] _retries Number of iterations until give up
+ protected: void WaitUntilEntitySpawn(const std::string &_name,
+ unsigned int _sleepEach,
+ int _retries);
+
+ /// \brief Wait for a number of ms. and attempts until the sensor is spawned
+ /// \param[in] _name Sensor name
+ /// \param[in] _sleepEach Number of milliseconds to sleep in each iteration
+ /// \param[in] _retries Number of iterations until give up
+ protected: void WaitUntilSensorSpawn(const std::string &_name,
+ unsigned int _sleepEach,
+ int _retries);
+
+ /// \brief Spawn a light.
+ /// \param[in] _name Name for the light.
+ /// \param[in] _size Type of light - "spot", "directional", or "point".
+ /// \param[in] _pos Position for the light.
+ /// \param[in] _rpy Roll, pitch, yaw for the light.
+ /// \param[in] _diffuse Diffuse color of the light.
+ /// \param[in] _specular Specular color of the light.
+ /// \param[in] _direction Direction of the light ("spot" and "directional").
+ /// \param[in] _attenuationRange Range of attenuation.
+ /// \param[in] _attenuationConstant Constant component of attenuation
+ /// \param[in] _attenuationLinear Linear component of attenuation
+ /// \param[in] _attenuationQuadratic Quadratic component of attenuation
+ /// \param[in] _spotInnerAngle Inner angle ("spot" only).
+ /// \param[in] _spotOuterAngle Outer angle ("spot" only).
+ /// \param[in] _spotFallOff Fall off ("spot" only).
+ /// \param[in] _castShadows True to cast shadows.
+ protected: void SpawnLight(const std::string &_name,
+ const std::string &_type,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ const common::Color &_diffuse = common::Color::White,
+ const common::Color &_specular = common::Color::White,
+ const math::Vector3 &_direction = -math::Vector3::UnitZ,
+ double _attenuationRange = 20,
+ double _attenuationConstant = 0.5,
+ double _attenuationLinear = 0.01,
+ double _attenuationQuadratic = 0.001,
+ double _spotInnerAngle = 0,
+ double _spotOuterAngle = 0,
+ double _spotFallOff = 0,
+ bool _castShadows = true);
+
+ /// \brief Spawn a cylinder
+ /// \param[in] _name Name for the model.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ protected: void SpawnCylinder(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _static = false);
+
+ /// \brief Spawn a sphere
+ /// \param[in] _name Name for the model.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ /// \param[in] _wait True to wait for the sphere to spawn before
+ /// returning.
+ protected: void SpawnSphere(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _wait = true, bool _static = false);
+
+ /// \brief Spawn a sphere
+ /// \param[in] _name Name for the model.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _cog Center of gravity.
+ /// \param[in] _radius Sphere radius.
+ /// \param[in] _static True to make the model static.
+ /// \param[in] _wait True to wait for the sphere to spawn before
+ /// returning.
+ protected: void SpawnSphere(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ const math::Vector3 &_cog, double _radius,
+ bool _wait = true, bool _static = false);
+
+ /// \brief Spawn a box.
+ /// \param[in] _name Name for the model.
+ /// \param[in] _size Size of the box.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ protected: void SpawnBox(const std::string &_name,
+ const math::Vector3 &_size, const math::Vector3 &_pos,
+ const math::Vector3 &_rpy, bool _static = false);
+
+ /// \brief Spawn a triangle mesh.
+ /// \param[in] _name Name for the model.
+ /// \param[in] _modelPath Path to the mesh file.
+ /// \param[in] _scale Scaling factor.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ protected: void SpawnTrimesh(const std::string &_name,
+ const std::string &_modelPath, const math::Vector3 &_scale,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _static = false);
+
+ /// \brief Spawn an empty link.
+ /// \param[in] _name Name for the model.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ protected: void SpawnEmptyLink(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _static = false);
+
+ /// \brief Spawn a model from file.
+ /// \param[in] _filename File to load a model from.
+ protected: void SpawnModel(const std::string &_filename);
+
+ /// \brief Send a factory message based on an SDF string.
+ /// \param[in] _sdf SDF string to publish.
+ protected: void SpawnSDF(const std::string &_sdf);
+
+ /// \brief Load a plugin.
+ /// \param[in] _filename Plugin filename to load.
+ /// \param[in] _name Name to associate with with the plugin.
+ protected: void LoadPlugin(const std::string &_filename,
+ const std::string &_name);
+
+ /// \brief Get a pointer to a model.
+ /// \param[in] _name Name of the model to get.
+ /// \return Pointer to the model, or NULL if the model was not found.
+ protected: physics::ModelPtr GetModel(const std::string &_name);
+
+ /// \brief Remove a model by name.
+ /// \param[in] _name Name of the model to remove.
+ protected: void RemoveModel(const std::string &_name);
+
+ /// \brief Remove a plugin.
+ /// \param[in] _name Name of the plugin to remove.
+ protected: void RemovePlugin(const std::string &_name);
+
+ /// \brief Get the current memory information.
+ /// \param[out] _resident Resident memory.
+ /// \param[out] _share Shared memory.
+ protected: void GetMemInfo(double &_resident, double &_share);
+
+ /// \brief Get unique string with a specified prefix.
+ /// \param[in] _prefix Prefix for unique string.
+ /// \return String with prefix and unique number as suffix.
+ protected: std::string GetUniqueString(const std::string &_prefix);
+
+ /// \brief Pointer the Gazebo server.
+ protected: Server *server;
+
+ /// \brief Pointer the thread the runs the server.
+ protected: boost::thread *serverThread;
+
+ /// \brief Pointer to a node for communication.
+ protected: transport::NodePtr node;
+
+ /// \brief Pose subscription.
+ protected: transport::SubscriberPtr poseSub;
+
+ /// \brief World statistics subscription.
+ protected: transport::SubscriberPtr statsSub;
+
+ /// \brief Factory publisher.
+ protected: transport::PublisherPtr factoryPub;
+
+ /// \brief Request publisher.
+ protected: transport::PublisherPtr requestPub;
+
+ /// \brief Map of received poses.
+ protected: std::map<std::string, math::Pose> poses;
+
+ /// \brief Mutex to protect data structures that store messages.
+ protected: boost::mutex receiveMutex;
+
+ /// \brief Image data
+ private: unsigned char **imgData;
+
+ /// \brief Increments when images are received.
+ private: int gotImage;
+
+ /// \brief Current simulation time, real time, and pause time.
+ protected: common::Time simTime, realTime, pauseTime;
+
+ /// \brief Current percent realtime.
+ private: double percentRealTime;
+
+ /// \brief True if paused.
+ private: bool paused;
+
+ /// \brief True if server is running.
+ private: bool serverRunning;
+
+ /// \brief Counter for unique name generation.
+ private: int uniqueCounter;
+ };
+} // namespace gazebo
#endif // define _GAZEBO_SERVER_FIXTURE_HH_
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 573f429..344cade 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -36,7 +36,6 @@ set(tests
concave_mesh.cc
contact_sensor.cc
dem.cc
- factory.cc
file_handling.cc
gripper.cc
gz_joint.cc
@@ -96,7 +95,6 @@ gz_build_tests(${tests})
if (ENABLE_TESTS_COMPILATION)
# Increase timeout, to account for model download time.
- set_tests_properties(${TEST_TYPE}_factory PROPERTIES TIMEOUT 500)
set_tests_properties(${TEST_TYPE}_physics PROPERTIES TIMEOUT 500)
# Add plugin dependency
@@ -112,6 +110,7 @@ gz_build_display_tests(${display_tests})
set(dri_tests
camera.cc
camera_sensor.cc
+ factory.cc
gpu_laser.cc
gz_camera.cc
heightmap.cc
@@ -134,9 +133,8 @@ gz_build_qt_tests(${qt_tests})
if (ENABLE_TESTS_COMPILATION AND VALID_DRI_DISPLAY)
# Increase timeout, to account for model download time.
+ set_tests_properties(${TEST_TYPE}_factory PROPERTIES TIMEOUT 500)
set_tests_properties(${TEST_TYPE}_pr2 PROPERTIES TIMEOUT 500)
-
- # Increase timeout, to account for model download time.
set_tests_properties(${TEST_TYPE}_pioneer2dx PROPERTIES TIMEOUT 500)
endif()
diff --git a/test/integration/contact_sensor.cc b/test/integration/contact_sensor.cc
index 2e3ec59..80d32bc 100644
--- a/test/integration/contact_sensor.cc
+++ b/test/integration/contact_sensor.cc
@@ -28,19 +28,88 @@ using namespace gazebo;
class ContactSensor : public ServerFixture,
public testing::WithParamInterface<const char*>
{
- public: void EmptyWorld(const std::string &_physicsEngine);
+ /// \brief Test multiple contact sensors on a single link.
+ /// \param[in] _physicsEngine Physics engine to use.
+ public: void MultipleSensors(const std::string &_physicsEngine);
public: void StackTest(const std::string &_physicsEngine);
public: void TorqueTest(const std::string &_physicsEngine);
+
+ /// \brief Callback for sensor subscribers in MultipleSensors test.
+ private: void Callback(const ConstContactsPtr &_msg);
};
-void ContactSensor::EmptyWorld(const std::string &_physicsEngine)
+unsigned int g_messageCount = 0;
+
+////////////////////////////////////////////////////////////////////////
+void ContactSensor::Callback(const ConstContactsPtr &/*_msg*/)
+{
+ g_messageCount++;
+}
+
+////////////////////////////////////////////////////////////////////////
+// Test having multiple contact sensors under a single link.
+// https://bitbucket.org/osrf/gazebo/issue/960
+////////////////////////////////////////////////////////////////////////
+void ContactSensor::MultipleSensors(const std::string &_physicsEngine)
{
- Load("worlds/empty.world", false, _physicsEngine);
+ Load("worlds/contact_sensors_multiple.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world();
+ ASSERT_TRUE(world != NULL);
+
+ const std::string contactSensorName1("box_contact");
+ const std::string contactSensorName2("box_contact2");
+
+ {
+ sensors::SensorPtr sensor1 = sensors::get_sensor(contactSensorName1);
+ sensors::ContactSensorPtr contactSensor1 =
+ boost::dynamic_pointer_cast<sensors::ContactSensor>(sensor1);
+ ASSERT_TRUE(contactSensor1 != NULL);
+ }
+
+ {
+ sensors::SensorPtr sensor2 = sensors::get_sensor(contactSensorName2);
+ sensors::ContactSensorPtr contactSensor2 =
+ boost::dynamic_pointer_cast<sensors::ContactSensor>(sensor2);
+ ASSERT_TRUE(contactSensor2 != NULL);
+ }
+
+ // There should be 5 topics advertising Contacts messages
+ std::list<std::string> topicsExpected;
+ std::string prefix = "/gazebo/default/";
+ topicsExpected.push_back(prefix+"physics/contacts");
+ topicsExpected.push_back(prefix+"sensor_box/link/box_contact/contacts");
+ topicsExpected.push_back(prefix+"sensor_box/link/box_contact");
+ topicsExpected.push_back(prefix+"sensor_box/link/box_contact2/contacts");
+ topicsExpected.push_back(prefix+"sensor_box/link/box_contact2");
+ topicsExpected.sort();
+
+ // Sleep to ensure transport topics are all advertised
+ common::Time::MSleep(100);
+ std::list<std::string> topics =
+ transport::getAdvertisedTopics("gazebo.msgs.Contacts");
+ topics.sort();
+ EXPECT_FALSE(topics.empty());
+ EXPECT_EQ(topics.size(), topicsExpected.size());
+ EXPECT_EQ(topics, topicsExpected);
+
+ // We should expect them all to publish.
+ for (auto const &topic : topics)
+ {
+ gzdbg << "Listening to " << topic << std::endl;
+ g_messageCount = 0;
+ transport::SubscriberPtr sub = this->node->Subscribe(topic,
+ &ContactSensor::Callback, this);
+
+ const unsigned int steps = 50;
+ world->Step(steps);
+ common::Time::MSleep(steps);
+ EXPECT_GT(g_messageCount, steps / 2);
+ }
}
-TEST_P(ContactSensor, EmptyWorld)
+TEST_P(ContactSensor, MultipleSensors)
{
- EmptyWorld(GetParam());
+ MultipleSensors(GetParam());
}
////////////////////////////////////////////////////////////////////////
diff --git a/test/integration/gripper.cc b/test/integration/gripper.cc
index a309d28..2587da4 100644
--- a/test/integration/gripper.cc
+++ b/test/integration/gripper.cc
@@ -22,6 +22,7 @@
#include "gazebo/physics/Model.hh"
#include "gazebo/physics/Joint.hh"
+using namespace gazebo;
class GripperTest : public ServerFixture
{
};
diff --git a/test/integration/joint_spawn.cc b/test/integration/joint_spawn.cc
index 677e78d..f35661f 100644
--- a/test/integration/joint_spawn.cc
+++ b/test/integration/joint_spawn.cc
@@ -71,14 +71,6 @@ class JointSpawningTest_RotationalWorld : public JointSpawningTest {};
void JointSpawningTest::SpawnJointTypes(const std::string &_physicsEngine,
const std::string &_jointType)
{
- /// \TODO: simbody not complete for this test
- if (_physicsEngine == "simbody") // &&
- // _jointType != "revolute" && _jointType != "prismatic")
- {
- gzerr << "Aborting test for Simbody, see issues #859, #861.\n";
- return;
- }
-
// Load an empty world
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -145,10 +137,10 @@ void JointSpawningTest::SpawnJointTypes(const std::string &_physicsEngine,
gzerr << "Skip tests for child world link cases "
<< "since DART does not allow joint with world as child. "
<< "Please see issue #914. "
- << "(https://bitbucket.org/osrf/gazebo/issue/914)\n";
- return;
+ << "(https://bitbucket.org/osrf/gazebo/issue/914)"
+ << std::endl;
}
-
+ else
{
gzdbg << "SpawnJoint " << _jointType << " world parent" << std::endl;
physics::JointPtr joint = SpawnJoint(_jointType, true, false);
@@ -173,13 +165,6 @@ void JointSpawningTest::SpawnJointTypes(const std::string &_physicsEngine,
void JointSpawningTest::SpawnJointRotational(const std::string &_physicsEngine,
const std::string &_jointType)
{
- /// \TODO: simbody not complete for this test
- if (_physicsEngine == "simbody" && _jointType != "revolute")
- {
- gzerr << "Aborting test for Simbody, see issue #859.\n";
- return;
- }
-
// Load an empty world
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -226,13 +211,6 @@ void JointSpawningTest::SpawnJointRotationalWorld(
const std::string &_physicsEngine,
const std::string &_jointType)
{
- /// \TODO: simbody not complete for this test
- if (_physicsEngine == "simbody") // && _jointType != "revolute")
- {
- gzerr << "Aborting test for Simbody, see issues #859, #861.\n";
- return;
- }
-
// Load an empty world
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -246,17 +224,6 @@ void JointSpawningTest::SpawnJointRotationalWorld(
physics::JointPtr joint;
for (unsigned int i = 0; i < 2; ++i)
{
- if (_physicsEngine == "dart" && i == 0)
- {
- // DART assumes that: (i) every link has its parent joint (ii) root link
- // is the only link that doesn't have parent link.
- // Child world link breaks dart for now. Do we need to support it?
- gzerr << "Skip tests for child world link cases "
- << "since DART does not allow joint with world as child. "
- << "Please see issue #914. "
- << "(https://bitbucket.org/osrf/gazebo/issue/914)\n";
- continue;
- }
bool worldChild = (i == 0);
bool worldParent = (i == 1);
std::string child = worldChild ? "world" : "child";
@@ -264,15 +231,30 @@ void JointSpawningTest::SpawnJointRotationalWorld(
gzdbg << "SpawnJoint " << _jointType << " "
<< child << " "
<< parent << std::endl;
+
+ if ((_physicsEngine == "dart" || _physicsEngine == "simbody")
+ && worldChild)
+ {
+ // These physics engines don't support world as a child link.
+ // simbody https://bitbucket.org/osrf/gazebo/issue/861
+ // dart https://bitbucket.org/osrf/gazebo/issue/914
+ gzerr << "Skip tests for child world link cases since "
+ << _physicsEngine
+ << " does not allow joint with world as child. "
+ << "Please see bitbucket issues #861, #914."
+ << std::endl;
+ continue;
+ }
+
joint = SpawnJoint(_jointType, worldChild, worldParent);
- EXPECT_TRUE(joint != NULL);
+ ASSERT_TRUE(joint != NULL);
physics::LinkPtr link;
if (!worldChild)
link = joint->GetChild();
else if (!worldParent)
link = joint->GetParent();
- EXPECT_TRUE(link != NULL);
+ ASSERT_TRUE(link != NULL);
math::Pose initialPose = link->GetWorldPose();
world->Step(100);
@@ -291,8 +273,9 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
ASSERT_TRUE(world != NULL);
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
- bool isOde = physics->GetType().compare("ode") == 0;
bool isBullet = physics->GetType().compare("bullet") == 0;
+ bool isDart = physics->GetType().compare("dart") == 0;
+ bool isSimbody = physics->GetType().compare("simbody") == 0;
double dt = physics->GetMaxStepSize();
if (_joint->HasType(physics::Base::HINGE2_JOINT) ||
@@ -308,6 +291,9 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
return;
}
+ // Reset world prior to testing SetVelocity
+ // This is needed for SimbodyUniversalJoint
+ world->Reset();
double velocityMagnitude = 1.0;
std::vector<double> velocities;
velocities.push_back(velocityMagnitude);
@@ -361,7 +347,7 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
}
// Test Coloumb friction
- if (isBullet && !_joint->HasType(physics::Base::HINGE_JOINT))
+ if (isBullet && _joint->HasType(physics::Base::UNIVERSAL_JOINT))
{
gzerr << "Skipping friction test for "
<< physics->GetType()
@@ -370,7 +356,7 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
<< " joint"
<< std::endl;
}
- else if (!isOde && !isBullet)
+ else if (isSimbody)
{
gzerr << "Skipping friction test for "
<< physics->GetType()
@@ -385,7 +371,7 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
// set friction to 1.0
const double friction = 1.0;
- _joint->SetParam("friction", _index, friction);
+ EXPECT_TRUE(_joint->SetParam("friction", _index, friction));
EXPECT_NEAR(_joint->GetParam("friction", _index), friction, g_tolerance);
for (unsigned int i = 0; i < 500; ++i)
@@ -409,6 +395,16 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
// Expect motion
EXPECT_GT(_joint->GetVelocity(_index), 0.2 * friction);
EXPECT_GT(_joint->GetAngle(_index).Radian(), 0.05 * friction);
+
+ // DART has problem with joint friction and joint limits
+ // https://github.com/dartsim/dart/issues/317
+ // Set friction back to zero to not interfere with other tests
+ // until this issue is resolved.
+ if (isDart)
+ {
+ EXPECT_TRUE(_joint->SetParam("friction", _index, 0.0));
+ EXPECT_NEAR(_joint->GetParam("friction", _index), 0.0, g_tolerance);
+ }
}
// SetHighStop
@@ -462,6 +458,12 @@ TEST_P(JointSpawningTest_All, SpawnJointTypes)
<< std::endl;
return;
}
+ if (physicsEngine == "simbody" && jointType == "revolute2")
+ {
+ gzerr << "Skip test, revolute2 not supported in simbody, see issue #859."
+ << std::endl;
+ return;
+ }
SpawnJointTypes(this->physicsEngine, this->jointType);
}
@@ -493,10 +495,10 @@ INSTANTIATE_TEST_CASE_P(TestRuns, JointSpawningTest_Rotational,
, "ball")));
// Skip prismatic, screw, and revolute2 because they allow translation
-// Skip universal because it can't be connected to world in bullet.
INSTANTIATE_TEST_CASE_P(TestRuns, JointSpawningTest_RotationalWorld,
::testing::Combine(PHYSICS_ENGINE_VALUES,
::testing::Values("revolute"
+ , "universal"
, "ball")));
int main(int argc, char **argv)
diff --git a/test/integration/joint_test.hh b/test/integration/joint_test.hh
index 07781b6..170909d 100644
--- a/test/integration/joint_test.hh
+++ b/test/integration/joint_test.hh
@@ -24,6 +24,7 @@
#include "test/ServerFixture.hh"
#include "gazebo/common/Time.hh"
+#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
using namespace gazebo;
@@ -31,9 +32,9 @@ using namespace gazebo;
typedef std::tr1::tuple<const char *, const char *> std_string2;
class JointTest : public ServerFixture,
- public ::testing::WithParamInterface<std_string2>
+ public ::testing::WithParamInterface<std_string2>
{
- protected: JointTest() : ServerFixture(), spawnCount(0)
+ protected: JointTest() : ServerFixture()
{
}
@@ -144,93 +145,75 @@ class JointTest : public ServerFixture,
/// \param[in] _opt Options for spawned model and joint.
public: physics::JointPtr SpawnJoint(const SpawnJointOptions &_opt)
{
- msgs::Factory msg;
- std::ostringstream modelStr;
- std::ostringstream modelName;
- modelName << "joint_model" << this->spawnCount++;
-
- modelStr
- << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << modelName.str() << "'>"
- << " <pose>" << _opt.modelPose << "</pose>";
+ msgs::Model msg;
+ std::string modelName = this->GetUniqueString("joint_model");
+ msg.set_name(modelName);
+ msgs::Set(msg.mutable_pose(), _opt.modelPose);
+
if (!_opt.worldParent)
{
- modelStr << " <link name='parent'>";
+ msg.add_link();
+ int linkCount = msg.link_size();
+ auto link = msg.mutable_link(linkCount-1);
+
+ link->set_name("parent");
if (!_opt.noLinkPose)
{
- modelStr << " <pose>" << _opt.parentLinkPose << "</pose>";
+ msgs::Set(link->mutable_pose(), _opt.parentLinkPose);
}
- modelStr << " </link>";
}
if (!_opt.worldChild)
{
- modelStr << " <link name='child'>";
+ msg.add_link();
+ int linkCount = msg.link_size();
+ auto link = msg.mutable_link(linkCount-1);
+
+ link->set_name("child");
if (!_opt.noLinkPose)
{
- modelStr << " <pose>" << _opt.childLinkPose << "</pose>";
+ msgs::Set(link->mutable_pose(), _opt.childLinkPose);
}
- modelStr << " </link>";
}
- modelStr
- << " <joint name='joint' type='" << _opt.type << "'>"
- << " <pose>" << _opt.jointPose << "</pose>";
+ msg.add_joint();
+ auto jointMsg = msg.mutable_joint(0);
+ jointMsg->set_name("joint");
+ jointMsg->set_type(msgs::ConvertJointType(_opt.type));
+ msgs::Set(jointMsg->mutable_pose(), _opt.jointPose);
if (_opt.worldParent)
- modelStr << " <parent>world</parent>";
+ {
+ jointMsg->set_parent("world");
+ }
else
- modelStr << " <parent>parent</parent>";
+ {
+ jointMsg->set_parent("parent");
+ }
if (_opt.worldChild)
- modelStr << " <child>world</child>";
+ {
+ jointMsg->set_child("world");
+ }
else
- modelStr << " <child>child</child>";
- modelStr
- << " <axis>"
- << " <xyz>" << _opt.axis << "</xyz>"
- << " <use_parent_model_frame>" << _opt.useParentModelFrame
- << " </use_parent_model_frame>"
- << " </axis>";
+ {
+ jointMsg->set_child("child");
+ }
+
+ {
+ auto axis = jointMsg->mutable_axis1();
+ msgs::Set(axis->mutable_xyz(), _opt.axis);
+ axis->set_use_parent_model_frame(_opt.useParentModelFrame);
+ }
// Hack: hardcode a second axis for universal joints
if (_opt.type == "universal")
{
- modelStr
- << " <axis2>"
- << " <xyz>" << math::Vector3(0, 1, 0) << "</xyz>"
- << " <use_parent_model_frame>" << _opt.useParentModelFrame
- << " </use_parent_model_frame>"
- << " </axis2>";
+ auto axis2 = jointMsg->mutable_axis2();
+ msgs::Set(axis2->mutable_xyz(), math::Vector3(0, 1, 0));
+ axis2->set_use_parent_model_frame(_opt.useParentModelFrame);
}
- modelStr
- << " </joint>"
- << "</model>";
-
- msg.set_sdf(modelStr.str());
- this->factoryPub->Publish(msg);
+ auto model = this->SpawnModel(msg);
physics::JointPtr joint;
- if (_opt.wait != common::Time::Zero)
- {
- common::Time wallStart = common::Time::GetWallTime();
- unsigned int waitCount = 0;
- while (_opt.wait > (common::Time::GetWallTime() - wallStart) &&
- !this->HasEntity(modelName.str()))
- {
- common::Time::MSleep(100);
- if (++waitCount % 10 == 0)
- {
- gzwarn << "Waiting " << waitCount / 10 << " seconds for "
- << _opt.type << " joint to spawn." << std::endl;
- }
- }
- if (this->HasEntity(modelName.str()) && waitCount >= 10)
- gzwarn << _opt.type << " joint has spawned." << std::endl;
+ if (model != NULL)
+ joint = model->GetJoint("joint");
- physics::WorldPtr world = physics::get_world("default");
- if (world != NULL)
- {
- physics::ModelPtr model = world->GetModel(modelName.str());
- if (model != NULL)
- joint = model->GetJoint("joint");
- }
- }
return joint;
}
@@ -239,8 +222,5 @@ class JointTest : public ServerFixture,
/// \brief Joint type for test.
protected: std::string jointType;
-
- /// \brief Count of spawned models, used to ensure unique model names.
- private: unsigned int spawnCount;
};
#endif
diff --git a/test/integration/model.cc b/test/integration/model.cc
index b03e90c..972256a 100644
--- a/test/integration/model.cc
+++ b/test/integration/model.cc
@@ -17,6 +17,7 @@
#include <string.h>
#include "ServerFixture.hh"
+using namespace gazebo;
class ModelTest : public ServerFixture
{
};
diff --git a/test/integration/ogre_log.cc b/test/integration/ogre_log.cc
index 38554a4..8f6203a 100644
--- a/test/integration/ogre_log.cc
+++ b/test/integration/ogre_log.cc
@@ -19,6 +19,7 @@
#include "ServerFixture.hh"
+using namespace gazebo;
class OgreLog : public ServerFixture
{
};
diff --git a/test/integration/physics_collision.cc b/test/integration/physics_collision.cc
index 0b932c4..774edba 100644
--- a/test/integration/physics_collision.cc
+++ b/test/integration/physics_collision.cc
@@ -22,7 +22,6 @@
using namespace gazebo;
-const double g_tolerance = 1e-4;
const double g_big = 1e29;
class PhysicsCollisionTest : public ServerFixture,
diff --git a/test/integration/physics_friction.cc b/test/integration/physics_friction.cc
index d2ac605..1b4d8f7 100644
--- a/test/integration/physics_friction.cc
+++ b/test/integration/physics_friction.cc
@@ -229,12 +229,6 @@ class PhysicsFrictionTest : public ServerFixture,
/// \param[in] _physicsEngine Physics engine to use.
public: void DirectionNaN(const std::string &_physicsEngine);
- /// \brief Test Link::GetWorldInertia* functions.
- /// \TODO: move the SpawnBox function to ServerFixture,
- /// and then move this test to a different file.
- /// \param[in] _physicsEngine Physics engine to use.
- public: void LinkGetWorldInertia(const std::string &_physicsEngine);
-
/// \brief Count of spawned models, used to ensure unique model names.
private: unsigned int spawnCount;
};
@@ -497,154 +491,6 @@ void PhysicsFrictionTest::DirectionNaN(const std::string &_physicsEngine)
}
/////////////////////////////////////////////////
-// LinkGetWorldInertia:
-// Spawn boxes and verify Link::GetWorldInertia* functions
-void PhysicsFrictionTest::LinkGetWorldInertia(const std::string &_physicsEngine)
-{
- // Load a blank world (no ground plane)
- Load("worlds/blank.world", true, _physicsEngine);
- physics::WorldPtr world = physics::get_world("default");
- ASSERT_TRUE(world != NULL);
-
- // Verify physics engine type
- physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
- ASSERT_TRUE(physics != NULL);
- EXPECT_EQ(physics->GetType(), _physicsEngine);
-
- // disable gravity
- physics->SetGravity(math::Vector3::Zero);
-
- // Box size
- double dx = 1.0;
- double dy = 4.0;
- double dz = 9.0;
- double mass = 10.0;
- double angle = M_PI / 3.0;
-
- const unsigned int testCases = 4;
- for (unsigned int i = 0; i < testCases; ++i)
- {
- // Set box size and position
- SpawnFrictionBoxOptions opt;
- opt.size.Set(dx, dy, dz);
- opt.mass = mass;
- opt.modelPose.pos.x = i * dz;
- opt.modelPose.pos.z = dz;
-
- // i=0: rotated model pose
- // expect inertial pose to match model pose
- if (i == 0)
- {
- opt.modelPose.rot.SetFromEuler(0.0, 0.0, angle);
- }
- // i=1: rotated link pose
- // expect inertial pose to match link pose
- else if (i == 1)
- {
- opt.linkPose.rot.SetFromEuler(0.0, 0.0, angle);
- }
- // i=2: rotated inertial pose
- // expect inertial pose to differ from link pose
- else if (i == 2)
- {
- opt.inertialPose.rot.SetFromEuler(0.0, 0.0, angle);
- }
- // i=3: offset inertial pose
- // expect inertial pose to differ from link pose
- else if (i == 3)
- {
- opt.inertialPose.pos.Set(1, 1, 1);
- }
-
- physics::ModelPtr model = SpawnBox(opt);
- ASSERT_TRUE(model != NULL);
-
- physics::LinkPtr link = model->GetLink();
- ASSERT_TRUE(link != NULL);
-
- EXPECT_EQ(model->GetWorldPose(), opt.modelPose);
- EXPECT_EQ(link->GetWorldPose(), opt.linkPose + opt.modelPose);
- EXPECT_EQ(link->GetWorldInertialPose(),
- opt.inertialPose + opt.linkPose + opt.modelPose);
-
- // i=0: rotated model pose
- // expect inertial pose to match model pose
- if (i == 0)
- {
- EXPECT_EQ(model->GetWorldPose(),
- link->GetWorldInertialPose());
- }
- // i=1: rotated link pose
- // expect inertial pose to match link pose
- else if (i == 1)
- {
- EXPECT_EQ(link->GetWorldPose(),
- link->GetWorldInertialPose());
- }
- // i=2: offset and rotated inertial pose
- // expect inertial pose to differ from link pose
- else if (i == 2)
- {
- EXPECT_EQ(link->GetWorldPose().pos,
- link->GetWorldInertialPose().pos);
- }
- // i=3: offset inertial pose
- // expect inertial pose to differ from link pose
- else if (i == 3)
- {
- EXPECT_EQ(link->GetWorldPose().pos + opt.inertialPose.pos,
- link->GetWorldInertialPose().pos);
- }
-
- // Expect rotated inertia matrix
- math::Matrix3 inertia = link->GetWorldInertiaMatrix();
- if (i == 3)
- {
- EXPECT_NEAR(inertia[0][0], 80.8333, 1e-4);
- EXPECT_NEAR(inertia[1][1], 68.3333, 1e-4);
- EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
- for (int row = 0; row < 3; ++row)
- for (int col = 0; col < 3; ++col)
- if (row != col)
- EXPECT_NEAR(inertia[row][col], 0.0, g_friction_tolerance);
- }
- else
- {
- EXPECT_NEAR(inertia[0][0], 71.4583, 1e-4);
- EXPECT_NEAR(inertia[1][1], 77.7083, 1e-4);
- EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
- EXPECT_NEAR(inertia[0][1], 5.4126, 1e-4);
- EXPECT_NEAR(inertia[1][0], 5.4126, 1e-4);
- EXPECT_NEAR(inertia[0][2], 0, g_friction_tolerance);
- EXPECT_NEAR(inertia[2][0], 0, g_friction_tolerance);
- EXPECT_NEAR(inertia[1][2], 0, g_friction_tolerance);
- EXPECT_NEAR(inertia[2][1], 0, g_friction_tolerance);
- }
-
- // For 0-2, apply torque and expect equivalent response
- if (i <= 2)
- {
- for (int step = 0; step < 50; ++step)
- {
- link->SetTorque(math::Vector3(100, 0, 0));
- world->Step(1);
- }
- if (_physicsEngine.compare("dart") == 0)
- {
- gzerr << "Dart fails this portion of the test (#1090)" << std::endl;
- }
- else
- {
- math::Vector3 vel = link->GetWorldAngularVel();
- EXPECT_NEAR(vel.x, 0.0703, g_friction_tolerance);
- EXPECT_NEAR(vel.y, -0.0049, g_friction_tolerance);
- EXPECT_NEAR(vel.z, 0.0000, g_friction_tolerance);
- }
- }
- }
-}
-
-/////////////////////////////////////////////////
TEST_P(PhysicsFrictionTest, FrictionDemo)
{
FrictionDemo(GetParam());
@@ -662,12 +508,6 @@ TEST_P(PhysicsFrictionTest, DirectionNaN)
DirectionNaN(GetParam());
}
-/////////////////////////////////////////////////
-TEST_P(PhysicsFrictionTest, LinkGetWorldInertia)
-{
- LinkGetWorldInertia(GetParam());
-}
-
INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsFrictionTest,
PHYSICS_ENGINE_VALUES);
diff --git a/test/integration/physics_inertia_ratio.cc b/test/integration/physics_inertia_ratio.cc
index 506ea6b..2bcc12f 100644
--- a/test/integration/physics_inertia_ratio.cc
+++ b/test/integration/physics_inertia_ratio.cc
@@ -29,7 +29,7 @@
using namespace gazebo;
const double g_angle_y_tol = 0.21;
-const double g_angle_z_tol = 0.21;
+const double g_angle_z_tol = 0.23;
class PhysicsTest : public ServerFixture,
public testing::WithParamInterface<const char*>
diff --git a/test/integration/physics_link.cc b/test/integration/physics_link.cc
index 1b7e972..e26d7e6 100644
--- a/test/integration/physics_link.cc
+++ b/test/integration/physics_link.cc
@@ -16,6 +16,8 @@
*/
#include <string.h>
+#include "gazebo/math/Vector3Stats.hh"
+#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "test/ServerFixture.hh"
#include "helper_physics_generator.hh"
@@ -27,16 +29,107 @@ const double g_tolerance = 1e-4;
class PhysicsLinkTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
+ /// \brief Test GetWorldAngularMomentum.
+ /// \param[in] _physicsEngine Type of physics engine to use.
+ public: void GetWorldAngularMomentum(const std::string &_physicsEngine);
+
/// \brief Test GetWorldEnergy* functions.
/// \param[in] _physicsEngine Type of physics engine to use.
public: void GetWorldEnergy(const std::string &_physicsEngine);
+ /// \brief Test Link::GetWorldInertia* functions.
+ /// \param[in] _physicsEngine Physics engine to use.
+ public: void GetWorldInertia(const std::string &_physicsEngine);
+
/// \brief Test velocity setting functions.
/// \param[in] _physicsEngine Type of physics engine to use.
public: void SetVelocity(const std::string &_physicsEngine);
};
/////////////////////////////////////////////////
+// GetWorldAngularMomentum:
+// Spawn box and verify Link::GetWorldAngularMomentum functions
+// Make dimensions unequal and give angular velocity that causes
+// gyroscopic tumbling.
+void PhysicsLinkTest::GetWorldAngularMomentum(const std::string &_physicsEngine)
+{
+ // Load a blank world (no ground plane)
+ Load("worlds/blank.world", true, _physicsEngine);
+ auto world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Verify physics engine type
+ auto physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+
+ // disable gravity
+ physics->SetGravity(math::Vector3::Zero);
+
+ physics::ModelPtr model;
+ {
+ // Box size
+ const double dx = 0.1;
+ const double dy = 0.4;
+ const double dz = 0.9;
+ const double mass = 10.0;
+
+ msgs::Model msgModel;
+ msgModel.set_name(this->GetUniqueString("model"));
+ msgs::AddBoxLink(msgModel, mass, math::Vector3(dx, dy, dz));
+ model = this->SpawnModel(msgModel);
+ }
+ ASSERT_TRUE(model != NULL);
+
+ // inertia matrix, recompute if dimensions change
+ const double Ixx = 0.80833333;
+ const double Iyy = 0.68333333;
+ const double Izz = 0.14166667;
+ const math::Matrix3 I0(Ixx, 0.0, 0.0
+ , 0.0, Iyy, 0.0
+ , 0.0, 0.0, Izz);
+
+ // Since Ixx > Iyy > Izz,
+ // angular velocity with large y component
+ // will cause gyroscopic tumbling
+ const math::Vector3 w0(1e-3, 1.5e0, 1.5e-2);
+ model->SetAngularVel(w0);
+
+ // Get link and verify inertia and initial velocity
+ auto link = model->GetLink();
+ ASSERT_TRUE(link != NULL);
+ ASSERT_EQ(w0, link->GetWorldAngularVel());
+ ASSERT_EQ(I0, link->GetWorldInertiaMatrix());
+
+ // Compute initial angular momentum
+ const math::Vector3 H0(I0 * w0);
+ ASSERT_EQ(H0, link->GetWorldAngularMomentum());
+ const double H0mag = H0.GetLength();
+
+ math::Vector3Stats angularMomentumError;
+ const std::string stat("maxAbs");
+ EXPECT_TRUE(angularMomentumError.InsertStatistic(stat));
+ const int steps = 5000;
+ for (int i = 0; i < steps; ++i)
+ {
+ world->Step(1);
+ math::Vector3 H = link->GetWorldAngularMomentum();
+ angularMomentumError.InsertData((H - H0) / H0mag);
+ }
+ if (_physicsEngine == "dart")
+ {
+ gzdbg << "dart has higher error for this test (see #1487), "
+ << "so a larger tolerance is used."
+ << std::endl;
+ EXPECT_LT(angularMomentumError.Mag().Map()[stat], g_tolerance * 1e3);
+ }
+ else
+ {
+ EXPECT_LT(angularMomentumError.Mag().Map()[stat], g_tolerance * 10);
+ }
+}
+
+/////////////////////////////////////////////////
void PhysicsLinkTest::GetWorldEnergy(const std::string &_physicsEngine)
{
Load("worlds/empty.world", true, _physicsEngine);
@@ -79,6 +172,165 @@ void PhysicsLinkTest::GetWorldEnergy(const std::string &_physicsEngine)
}
/////////////////////////////////////////////////
+// GetWorldInertia:
+// Spawn boxes and verify Link::GetWorldInertia* functions
+void PhysicsLinkTest::GetWorldInertia(const std::string &_physicsEngine)
+{
+ // Load a blank world (no ground plane)
+ Load("worlds/blank.world", true, _physicsEngine);
+ auto world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Verify physics engine type
+ auto physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+
+ // disable gravity
+ physics->SetGravity(math::Vector3::Zero);
+
+ // Box size
+ const double dx = 1.0;
+ const double dy = 4.0;
+ const double dz = 9.0;
+ const double mass = 10.0;
+ const double angle = M_PI / 3.0;
+
+ const unsigned int testCases = 4;
+ for (unsigned int i = 0; i < testCases; ++i)
+ {
+ // Use msgs::AddBoxLink
+ msgs::Model msgModel;
+ math::Pose modelPose, linkPose, inertialPose;
+
+ msgModel.set_name(this->GetUniqueString("model"));
+ msgs::AddBoxLink(msgModel, mass, math::Vector3(dx, dy, dz));
+ modelPose.pos.x = i * dz;
+ modelPose.pos.z = dz;
+
+ // i=0: rotated model pose
+ // expect inertial pose to match model pose
+ if (i == 0)
+ {
+ modelPose.rot.SetFromEuler(0.0, 0.0, angle);
+ }
+ // i=1: rotated link pose
+ // expect inertial pose to match link pose
+ else if (i == 1)
+ {
+ linkPose.rot.SetFromEuler(0.0, 0.0, angle);
+ }
+ // i=2: rotated inertial pose
+ // expect inertial pose to differ from link pose
+ else if (i == 2)
+ {
+ inertialPose.rot.SetFromEuler(0.0, 0.0, angle);
+ }
+ // i=3: offset inertial pose
+ // expect inertial pose to differ from link pose
+ else if (i == 3)
+ {
+ inertialPose.pos.Set(1, 1, 1);
+ }
+
+ {
+ auto msgLink = msgModel.mutable_link(0);
+ auto msgInertial = msgLink->mutable_inertial();
+
+ msgs::Set(msgModel.mutable_pose(), modelPose);
+ msgs::Set(msgLink->mutable_pose(), linkPose);
+ msgs::Set(msgInertial->mutable_pose(), inertialPose);
+ }
+
+ auto model = this->SpawnModel(msgModel);
+ ASSERT_TRUE(model != NULL);
+
+ auto link = model->GetLink();
+ ASSERT_TRUE(link != NULL);
+
+ EXPECT_EQ(model->GetWorldPose(), modelPose);
+ EXPECT_EQ(link->GetWorldPose(), linkPose + modelPose);
+ EXPECT_EQ(link->GetWorldInertialPose(),
+ inertialPose + linkPose + modelPose);
+
+ // i=0: rotated model pose
+ // expect inertial pose to match model pose
+ if (i == 0)
+ {
+ EXPECT_EQ(model->GetWorldPose(),
+ link->GetWorldInertialPose());
+ }
+ // i=1: rotated link pose
+ // expect inertial pose to match link pose
+ else if (i == 1)
+ {
+ EXPECT_EQ(link->GetWorldPose(),
+ link->GetWorldInertialPose());
+ }
+ // i=2: offset and rotated inertial pose
+ // expect inertial pose to differ from link pose
+ else if (i == 2)
+ {
+ EXPECT_EQ(link->GetWorldPose().pos,
+ link->GetWorldInertialPose().pos);
+ }
+ // i=3: offset inertial pose
+ // expect inertial pose to differ from link pose
+ else if (i == 3)
+ {
+ EXPECT_EQ(link->GetWorldPose().pos + inertialPose.pos,
+ link->GetWorldInertialPose().pos);
+ }
+
+ // Expect rotated inertia matrix
+ math::Matrix3 inertia = link->GetWorldInertiaMatrix();
+ if (i == 3)
+ {
+ EXPECT_NEAR(inertia[0][0], 80.8333, 1e-4);
+ EXPECT_NEAR(inertia[1][1], 68.3333, 1e-4);
+ EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
+ for (int row = 0; row < 3; ++row)
+ for (int col = 0; col < 3; ++col)
+ if (row != col)
+ EXPECT_NEAR(inertia[row][col], 0.0, g_tolerance);
+ }
+ else
+ {
+ EXPECT_NEAR(inertia[0][0], 71.4583, 1e-4);
+ EXPECT_NEAR(inertia[1][1], 77.7083, 1e-4);
+ EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
+ EXPECT_NEAR(inertia[0][1], 5.4126, 1e-4);
+ EXPECT_NEAR(inertia[1][0], 5.4126, 1e-4);
+ EXPECT_NEAR(inertia[0][2], 0, g_tolerance);
+ EXPECT_NEAR(inertia[2][0], 0, g_tolerance);
+ EXPECT_NEAR(inertia[1][2], 0, g_tolerance);
+ EXPECT_NEAR(inertia[2][1], 0, g_tolerance);
+ }
+
+ // For 0-2, apply torque and expect equivalent response
+ if (i <= 2)
+ {
+ for (int step = 0; step < 50; ++step)
+ {
+ link->SetTorque(math::Vector3(100, 0, 0));
+ world->Step(1);
+ }
+ if (_physicsEngine.compare("dart") == 0)
+ {
+ gzerr << "Dart fails this portion of the test (#1090)" << std::endl;
+ }
+ else
+ {
+ math::Vector3 vel = link->GetWorldAngularVel();
+ EXPECT_NEAR(vel.x, 0.0703, g_tolerance);
+ EXPECT_NEAR(vel.y, -0.0049, g_tolerance);
+ EXPECT_NEAR(vel.z, 0.0000, g_tolerance);
+ }
+ }
+ }
+}
+
+/////////////////////////////////////////////////
void PhysicsLinkTest::SetVelocity(const std::string &_physicsEngine)
{
Load("worlds/empty.world", true, _physicsEngine);
@@ -154,12 +406,24 @@ void PhysicsLinkTest::SetVelocity(const std::string &_physicsEngine)
}
/////////////////////////////////////////////////
+TEST_P(PhysicsLinkTest, GetWorldAngularMomentum)
+{
+ GetWorldAngularMomentum(GetParam());
+}
+
+/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, GetWorldEnergy)
{
GetWorldEnergy(GetParam());
}
/////////////////////////////////////////////////
+TEST_P(PhysicsLinkTest, GetWorldInertia)
+{
+ GetWorldInertia(GetParam());
+}
+
+/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, SetVelocity)
{
SetVelocity(GetParam());
diff --git a/test/integration/polyline.cc b/test/integration/polyline.cc
index 4cfdd2d..d8acb14 100644
--- a/test/integration/polyline.cc
+++ b/test/integration/polyline.cc
@@ -19,6 +19,7 @@
#include "gazebo/msgs/msgs.hh"
#include "helper_physics_generator.hh"
+using namespace gazebo;
class PolylineTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
diff --git a/test/integration/pr2.cc b/test/integration/pr2.cc
index 7c87c34..ebef177 100644
--- a/test/integration/pr2.cc
+++ b/test/integration/pr2.cc
@@ -153,8 +153,10 @@ void PR2Test::StaticPR2(std::string _physicsEngine)
{
if (_physicsEngine == "simbody")
{
- gzerr << "Abort test since simbody does not support screw joints in PR2, "
- << "Please see issue #857.\n";
+ gzerr << "Abort test since simbody does not support static models "
+ << "with pose offsets, "
+ << "please see issue #860."
+ << std::endl;
return;
}
if (_physicsEngine == "dart")
diff --git a/test/integration/sensor.cc b/test/integration/sensor.cc
index 78c335c..3d08a86 100644
--- a/test/integration/sensor.cc
+++ b/test/integration/sensor.cc
@@ -17,6 +17,7 @@
#include <string.h>
#include "ServerFixture.hh"
+using namespace gazebo;
class SensorTest : public ServerFixture
{
};
diff --git a/test/integration/speed.cc b/test/integration/speed.cc
index 692694a..29c9579 100644
--- a/test/integration/speed.cc
+++ b/test/integration/speed.cc
@@ -21,10 +21,24 @@ using namespace gazebo;
class SpeedTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
+ /// \brief Spawn 500 spheres into empty.world and
+ /// verify that real-time factor is fast enough.
+ /// \param[in] _physicsEngine Physics engine to use.
public: void BallTest(const std::string &_physicsEngine);
+
+ /// \brief Spawn 500 spheres into shapes.world and
+ /// verify that real-time factor is fast enough.
+ /// \param[in] _physicsEngine Physics engine to use.
public: void ShapesWorld(const std::string &_physicsEngine);
+
+ /// \brief Unthrottle real-time update rate and call World::Step
+ /// in an empty world.
+ /// Verify that it goes at least 2 times faster than real-time.
+ /// \param[in] _physicsEngine Physics engine to use.
+ public: void UnthrottledStep(const std::string &_physicsEngine);
};
+//////////////////////////////////////////////////
void SpeedTest::BallTest(const std::string &_physicsEngine)
{
Load("worlds/empty.world", false, _physicsEngine);
@@ -65,6 +79,7 @@ TEST_P(SpeedTest, BallTest)
BallTest(GetParam());
}
+//////////////////////////////////////////////////
void SpeedTest::ShapesWorld(const std::string &_physicsEngine)
{
Load("worlds/shapes.world", false, _physicsEngine);
@@ -103,6 +118,37 @@ TEST_P(SpeedTest, ShapesWorld)
ShapesWorld(GetParam());
}
+//////////////////////////////////////////////////
+void SpeedTest::UnthrottledStep(const std::string &_physicsEngine)
+{
+ Load("worlds/empty.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Unthrottle physics updates
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ physics->SetRealTimeUpdateRate(0.0);
+ double dt = physics->GetMaxStepSize();
+
+ const unsigned int steps = 2000;
+ common::Time startTime = common::Time::GetWallTime();
+ for (unsigned int i = 0; i < steps; ++i)
+ {
+ world->Step(1);
+ }
+ common::Time runTime = common::Time::GetWallTime() - startTime;
+
+ double realTimeFactor = dt * steps / runTime.Double();
+ gzdbg << "realTimeFactor " << realTimeFactor << std::endl;
+ EXPECT_GT(realTimeFactor, 2.0);
+}
+
+TEST_P(SpeedTest, UnthrottledStep)
+{
+ UnthrottledStep(GetParam());
+}
+
INSTANTIATE_TEST_CASE_P(PhysicsEngines, SpeedTest, PHYSICS_ENGINE_VALUES);
int main(int argc, char **argv)
diff --git a/test/integration/world.cc b/test/integration/world.cc
index dbbc429..ee3234e 100644
--- a/test/integration/world.cc
+++ b/test/integration/world.cc
@@ -17,6 +17,7 @@
#include "ServerFixture.hh"
#include "gazebo/physics/physics.hh"
+using namespace gazebo;
class WorldTest : public ServerFixture
{
};
diff --git a/test/integration/world_clone.cc b/test/integration/world_clone.cc
index a3e5d23..cf109eb 100644
--- a/test/integration/world_clone.cc
+++ b/test/integration/world_clone.cc
@@ -19,6 +19,7 @@
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
+using namespace gazebo;
class WorldClone : public ServerFixture
{
};
diff --git a/gazebo/common/ffmpeg_inc.h b/test/regression/351_world_step.cc
similarity index 56%
copy from gazebo/common/ffmpeg_inc.h
copy to test/regression/351_world_step.cc
index 8933ef0..77abf74 100644
--- a/gazebo/common/ffmpeg_inc.h
+++ b/test/regression/351_world_step.cc
@@ -15,23 +15,24 @@
*
*/
-#ifndef _GAZEBO_FFMPEG_INC_HH_
-#define _GAZEBO_FFMPEG_INC_HH_
+#include <gtest/gtest.h>
+#include <gazebo/rendering/rendering.hh>
+#include "ServerFixture.hh"
-#pragma GCC system_header
+using namespace gazebo;
-#ifdef HAVE_FFMPEG
-#ifndef INT64_C
-#define INT64_C(c) (c ## LL)
-#define UINT64_C(c) (c ## ULL)
-#endif
+class Issue351Test : public ServerFixture
+{
+};
-extern "C" {
-#include <libavcodec/avcodec.h>
-#include <libavformat/avformat.h>
-#include <libavutil/opt.h>
-#include <libswscale/swscale.h>
-}
-#endif // ifdef HAVE_FFMPEG
+/////////////////////////////////////////////////
+// \brief Test for issue #351
+TEST_F(Issue351Test, WorldStep)
+{
+ Load("worlds/world_step.world", true);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
-#endif // ifndef _GAZEBO_FFMPEG_INC_HH
+ // Take 500 steps; it passes if it doesn't seg-fault
+ world->Step(500);
+}
diff --git a/test/regression/602_unsubscribe_segfault.cc b/test/regression/602_unsubscribe_segfault.cc
new file mode 100644
index 0000000..f8ba67b
--- /dev/null
+++ b/test/regression/602_unsubscribe_segfault.cc
@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 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 "gazebo/physics/World.hh"
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/JointController.hh"
+#include "gazebo/common/PID.hh"
+#include "test/ServerFixture.hh"
+#include "test_config.h"
+
+using namespace gazebo;
+class Issue602Test : public ServerFixture
+{
+ /// \brief Test for Unsubscribe before delete subscriber.
+ public: void UnsubscribeTest();
+
+ /// \brief Callback for sensor subscribers in MultipleSensors test.
+ /// \param[in] _msg World Statistics message.
+ public: void Callback(const ConstContactsPtr &_msg);
+};
+
+unsigned int g_messageCount = 0;
+
+////////////////////////////////////////////////////////////////////////
+void Issue602Test::Callback(const ConstContactsPtr &/*_msg*/)
+{
+ g_messageCount++;
+}
+
+/////////////////////////////////////////////////
+TEST_F(Issue602Test, Unsubscribe)
+{
+ UnsubscribeTest();
+}
+
+/////////////////////////////////////////////////
+void Issue602Test::UnsubscribeTest()
+{
+ Load("worlds/contact_sensors_multiple.world", true);
+ physics::WorldPtr world = physics::get_world();
+ ASSERT_TRUE(world != NULL);
+
+ const std::string contactSensorName1("box_contact");
+ const std::string contactSensorName2("box_contact2");
+
+ {
+ sensors::SensorPtr sensor1 = sensors::get_sensor(contactSensorName1);
+ sensors::ContactSensorPtr contactSensor1 =
+ boost::dynamic_pointer_cast<sensors::ContactSensor>(sensor1);
+ ASSERT_TRUE(contactSensor1 != NULL);
+ }
+
+ {
+ sensors::SensorPtr sensor2 = sensors::get_sensor(contactSensorName2);
+ sensors::ContactSensorPtr contactSensor2 =
+ boost::dynamic_pointer_cast<sensors::ContactSensor>(sensor2);
+ ASSERT_TRUE(contactSensor2 != NULL);
+ }
+
+ // Sleep to ensure transport topics are all advertised
+ common::Time::MSleep(100);
+ auto topics = transport::getAdvertisedTopics("gazebo.msgs.Contacts");
+ EXPECT_FALSE(topics.empty());
+ EXPECT_GE(topics.size(), 4u);
+
+ // We should expect them all to publish.
+ for (auto const &topic : topics)
+ {
+ gzdbg << "Listening to " << topic << std::endl;
+ g_messageCount = 0;
+ transport::SubscriberPtr sub = this->node->Subscribe(topic,
+ &Issue602Test::Callback, this);
+
+ const unsigned int steps = 50;
+ world->Step(steps);
+ common::Time::MSleep(steps);
+
+ sub->Unsubscribe();
+ common::Time::MSleep(steps);
+ }
+}
+
+/////////////////////////////////////////////////
+/// Main
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/regression/846_typo_in_camera.cc b/test/regression/846_typo_in_camera.cc
index 8945b02..b93b0e4 100644
--- a/test/regression/846_typo_in_camera.cc
+++ b/test/regression/846_typo_in_camera.cc
@@ -19,6 +19,7 @@
#include <gazebo/rendering/rendering.hh>
#include "ServerFixture.hh"
+using namespace gazebo;
class Issue846Test : public ServerFixture
{
};
diff --git a/test/regression/CMakeLists.txt b/test/regression/CMakeLists.txt
index 6050f87..7824736 100644
--- a/test/regression/CMakeLists.txt
+++ b/test/regression/CMakeLists.txt
@@ -22,7 +22,9 @@ if(HAVE_DART)
endif()
set(tests
+ 351_world_step.cc
494_joint_axis_frame.cc
+ 602_unsubscribe_segfault.cc
624_collision_world_pose.cc
876_random_number_generator.cc
940_force_torque_sensor_frame.cc
diff --git a/test/worlds/contact_sensors_multiple.world b/test/worlds/contact_sensors_multiple.world
new file mode 100644
index 0000000..001b500
--- /dev/null
+++ b/test/worlds/contact_sensors_multiple.world
@@ -0,0 +1,67 @@
+<sdf version='1.5'>
+ <world name='default'>
+ <!-- include sun and ground plane -->
+ <include><uri>model://sun</uri></include>
+ <include><uri>model://ground_plane</uri></include>
+
+ <model name="sensor_box">
+ <static>true</static>
+ <link name="link">
+ <pose>0 0 0.251 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>4 4 0.5</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>4 4 0.5</size>
+ </box>
+ </geometry>
+ <material><script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script></material>
+ </visual>
+ <collision name="collision2">
+ <pose>0 0 1 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>4 4 0.5</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual2">
+ <pose>0 0 1 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>4 4 0.5</size>
+ </box>
+ </geometry>
+ <material><script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script></material>
+ </visual>
+ <sensor name="box_contact" type="contact">
+ <always_on>true</always_on>
+ <update_rate>1000</update_rate>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ <sensor name="box_contact2" type="contact">
+ <always_on>true</always_on>
+ <update_rate>1000</update_rate>
+ <contact>
+ <collision>collision2</collision>
+ </contact>
+ </sensor>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/test/worlds/ray_test.world b/test/worlds/ray_test.world
index ee3d7c1..0dc5702 100644
--- a/test/worlds/ray_test.world
+++ b/test/worlds/ray_test.world
@@ -58,6 +58,9 @@
</box>
</geometry>
</collision>
+ <sensor name="box_imu_sensor" type="imu">
+ <always_on>1</always_on>
+ </sensor>
</link>
</model>
diff --git a/test/worlds/revolute_joint_test.world b/test/worlds/revolute_joint_test.world
index e0640bf..56fb750 100644
--- a/test/worlds/revolute_joint_test.world
+++ b/test/worlds/revolute_joint_test.world
@@ -35,7 +35,6 @@
<uri>model://double_pendulum_with_base</uri>
<name>pendulum_225deg</name>
<pose>1.48 -1.48 0 0 0 -2.3562</pose>
- <pose>0 -2.1 0 0 0</pose>
</include>
<include>
<uri>model://double_pendulum_with_base</uri>
diff --git a/test/worlds/world_step.world b/test/worlds/world_step.world
new file mode 100644
index 0000000..029df77
--- /dev/null
+++ b/test/worlds/world_step.world
@@ -0,0 +1,325 @@
+<sdf version="1.5">
+ <world name="default">
+ <physics type="ode">
+ <gravity>0.000000 0.000000 -9.810000</gravity>
+ <ode>
+ <solver>
+ <type>world</type>
+ <iters>250</iters>
+ <precon_iters>0</precon_iters>
+ <sor>1.400000</sor>
+ </solver>
+ <constraints>
+ <cfm>0.000000</cfm>
+ <erp>0.200000</erp>
+ <contact_max_correcting_vel>0.000000</contact_max_correcting_vel>
+ <contact_surface_layer>0.00000</contact_surface_layer>
+ </constraints>
+ </ode>
+ <real_time_update_rate>1000.000000</real_time_update_rate>
+ <max_step_size>0.001000</max_step_size>
+ </physics>
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>100</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+ <model name="double_pendulum_with_base">
+ <link name="base">
+ <inertial>
+ <mass>100</mass>
+ </inertial>
+ <visual name="vis_plate_on_ground">
+ <pose>0 0 0.01 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.8</radius>
+ <length>0.02</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name="vis_pole">
+ <pose>-0.275 0 1.1 0 0 0</pose>
+ <geometry>
+ <box><size>0.2 0.2 2.2</size></box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name="col_plate_on_ground">
+ <pose>0 0 0.01 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.8</radius>
+ <length>0.02</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <collision name="col_pole">
+ <pose>-0.275 0 1.1 0 0 0</pose>
+ <geometry>
+ <box><size>0.2 0.2 2.2</size></box>
+ </geometry>
+ </collision>
+ </link>
+ <!-- upper link, length 1, IC -90 degrees -->
+ <link name="upper_link">
+ <pose>0 0 2.1 -1.5708 0 0</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0 0 0.5 0 0 0</pose>
+ </inertial>
+ <visual name="vis_upper_joint">
+ <pose>-0.05 0 0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.3</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name="vis_lower_joint">
+ <pose>0 0 1.0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.2</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name="vis_cylinder">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.9</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name="col_upper_joint">
+ <pose>-0.05 0 0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.3</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <collision name="col_lower_joint">
+ <pose>0 0 1.0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.2</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <collision name="col_cylinder">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.9</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ </link>
+ <!-- lower link, length 1, IC ~-120 degrees more -->
+ <link name="lower_link">
+ <pose>0.25 1.0 2.1 -2 0 0</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0 0 0.5 0 0 0</pose>
+ </inertial>
+ <visual name="vis_lower_joint">
+ <pose>0 0 0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.08</radius>
+ <length>0.3</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name="vis_cylinder">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.9</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name="col_lower_joint">
+ <pose>0 0 0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.08</radius>
+ <length>0.3</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <collision name="col_cylinder">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.9</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ </link>
+ <!-- pin joint for upper link, at origin of upper link -->
+ <joint name="upper_joint" type="revolute">
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.0 0 0</xyz>
+ </axis>
+ </joint>
+ <!-- pin joint for lower link, at origin of child link -->
+ <joint name="lower_joint" type="revolute">
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.0 0 0</xyz>
+ </axis>
+ </joint>
+ </model>
+ <model name="sphere">
+ <pose>0 1.5 4.5 0 0 0</pose>
+ <allow_auto_disable>false</allow_auto_disable>
+ <link name="link">
+ <inertial>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0.3</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ <mass>1.0</mass>
+ </inertial>
+ <velocity_decay>
+ <linear>0.0</linear>
+ <angular>0.0</angular>
+ </velocity_decay>
+ <collision name="collision">
+ <geometry>
+ <sphere>
+ <radius>0.5</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>0.5</mu>
+ <mu2>1</mu2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <max_vel>0</max_vel>
+ <min_depth>0</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <sphere>
+ <radius>0.5</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>Gazebo/WoodPallet</script>
+ </material>
+ </visual>
+ </link>
+ </model>
+ <light type="directional" name="sun">
+ <pose>0 0 10 0 0 0</pose>
+ <diffuse>8 .8 .8 1</diffuse>
+ <specular>.1 .1 .1 1</specular>
+ <attenuation>
+ <range>10</range>
+ <linear>0.01</linear>
+ <constant>0.8</constant>
+ <quadratic>0.0</quadratic>
+ </attenuation>
+ <direction>0 .5 -.5</direction>
+ <cast_shadows>true</cast_shadows>
+ </light>
+ </world>
+</sdf>
diff --git a/tools/check_test_ran.py b/tools/check_test_ran.py
index 48f8639..0ef7db7 100755
--- a/tools/check_test_ran.py
+++ b/tools/check_test_ran.py
@@ -97,7 +97,7 @@ def check_main():
else:
pid = m.group(1)
print("killing gzserver with pid %s" % (pid))
- subprocess.call(["kill", "%s" % (pid)])
+ subprocess.call(["kill", "-9", "%s" % (pid)])
print("Checking for test results in %s"%test_file)
diff --git a/tools/gz_TEST.cc b/tools/gz_TEST.cc
index 25ca807..534311d 100644
--- a/tools/gz_TEST.cc
+++ b/tools/gz_TEST.cc
@@ -201,8 +201,6 @@ TEST_F(gzTest, Joint)
{
init();
- std::string expectedStr;
-
std::string helpOutput = custom_exec_str("gz help joint");
EXPECT_NE(helpOutput.find("gz joint"), std::string::npos);
@@ -621,8 +619,8 @@ TEST_F(gzTest, SDF)
descSums["1.0"] = "5235eb8464a96505c2a31fe96327d704e45c9cc4";
descSums["1.2"] = "27973b2542d7a0f7582a615b245d81797718c89a";
descSums["1.3"] = "30ffce1c662c17185d23f30ef3af5c110d367e10";
- descSums["1.4"] = "9a55c2992b532e0dd4f9a5d5b86cd4c6210d9902";
- descSums["1.5"] = "a64da1ba2cec921a8b8e446ef01a862dc3e38796";
+ descSums["1.4"] = "eb1798699f1926e6e75083970528c598bfa6d7f7";
+ descSums["1.5"] = "0c056ee3d5fdfb87f4d109d5161cac69a0387839";
// Test each descSum
for (std::map<std::string, std::string>::iterator iter = descSums.begin();
@@ -640,8 +638,8 @@ TEST_F(gzTest, SDF)
docSums["1.0"] = "4cf955ada785adf72503744604ffadcdf13ec0d2";
docSums["1.2"] = "f84c1cf1b1ba04ab4859e96f6aea881134fb5a9b";
docSums["1.3"] = "f3dd699687c8922710e4492aadedd1c038d678c1";
- docSums["1.4"] = "9627b1deb0c0437ac48c1c1cbd0e9800b0327b76";
- docSums["1.5"] = "e67df38350bed73372cb0072cfd32c8cf07f5bf9";
+ docSums["1.4"] = "31082d3b9fda88b1ac25588323e31c305937a548";
+ docSums["1.5"] = "1345996b27eb06f8d18cc4584b37f8ca4a6e8e69";
// Test each docSum
for (std::map<std::string, std::string>::iterator iter = docSums.begin();
@@ -672,7 +670,7 @@ TEST_F(gzTest, SDF)
std::string output =
custom_exec_str(std::string("gz sdf -p ") + path.string());
std::string shasum = gazebo::common::get_sha1<std::string>(output);
- EXPECT_EQ(shasum, "381ce9100dd002bcf7e6f9019e33d478a37ab6f1");
+ EXPECT_EQ(shasum, "64dcab58596d2d2dbc596a37ee198bda58cc2b8a");
}
path = PROJECT_BINARY_PATH;
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/gazebo.git
More information about the debian-science-commits
mailing list