[gazebo] 02/09: Imported Upstream version 6.5.0
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Wed Oct 28 00:24:31 UTC 2015
This is an automated email from the git hooks/post-receive script.
jrivero-guest pushed a commit to branch master
in repository gazebo.
commit c2e3c8cc797c85a180a91aaa3e5abb66f459c389
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Mon Oct 26 13:52:23 2015 +0100
Imported Upstream version 6.5.0
---
.hg_archival.txt | 6 +-
CMakeLists.txt | 6 +-
Changelog.md | 46 +++
cmake/GazeboTestUtils.cmake | 12 +-
cmake/SearchForStuff.cmake | 2 +-
cmake/gazebo-config.cmake.in | 63 +++-
cmake/gazebo_config.h.in | 1 +
cmake/upload_doc.sh.in | 36 ++
deps/opende/CMakeLists.txt | 7 +
deps/opende/include/ode/objects.h | 3 +-
deps/opende/src/joints/gearbox.cpp | 3 +
deps/opende/src/step.cpp | 38 +-
.../opende/src/step_bullet_inc.h | 33 +-
deps/opende/src/step_bullet_lemke_wrapper.cpp | 74 ++++
.../opende/src/step_bullet_lemke_wrapper.h | 27 +-
deps/opende/src/step_bullet_pgs_wrapper.cpp | 69 ++++
.../opende/src/step_bullet_pgs_wrapper.h | 24 +-
examples/plugins/CMakeLists.txt | 14 -
examples/plugins/animate_joints/CMakeLists.txt | 11 +-
.../GUIExampleSpawnWidget.cc | 32 +-
examples/plugins/hello_world/CMakeLists.txt | 2 +-
examples/plugins/mainwindow_example/CMakeLists.txt | 5 +-
.../mainwindow_example/MainWindowExampleWidget.hh | 6 +-
examples/plugins/model_move/CMakeLists.txt | 13 +-
examples/plugins/model_push/CMakeLists.txt | 2 +-
examples/plugins/model_push/model_push.world | 2 +-
examples/plugins/parameters/CMakeLists.txt | 8 +-
examples/plugins/projector/CMakeLists.txt | 8 +-
examples/plugins/system_gui_plugin/CMakeLists.txt | 2 +-
examples/stand_alone/actuator/CMakeLists.txt | 12 +-
examples/stand_alone/animated_box/CMakeLists.txt | 13 +-
.../stand_alone/clone_simulation/CMakeLists.txt | 8 +-
examples/stand_alone/custom_main/CMakeLists.txt | 10 +-
.../custom_main_pkgconfig/CMakeLists.txt | 9 +-
examples/stand_alone/listener/CMakeLists.txt | 8 +-
examples/stand_alone/publisher/CMakeLists.txt | 10 +-
examples/stand_alone/publisher/publisher.cc | 2 +-
examples/stand_alone/test_fixture/CMakeLists.txt | 6 +-
examples/stand_alone/transporter/CMakeLists.txt | 7 +-
examples/stand_alone/transporter/transporter.cc | 6 +-
gazebo/Master.cc | 1 +
gazebo/common/CMakeLists.txt | 1 +
gazebo/common/Event_TEST.cc | 1 +
gazebo/common/ModelDatabase.cc | 2 +
gazebo/common/ModelDatabase.hh | 1 +
gazebo/common/ModelDatabasePrivate.hh | 1 +
gazebo/common/MouseEvent.cc | 18 +
gazebo/common/MouseEvent.hh | 9 +
gazebo/common/MouseEvent_TEST.cc | 278 +++++++++++++++
gazebo/common/SphericalCoordinates.cc | 384 +++++++++++++++++----
gazebo/common/SphericalCoordinates.hh | 55 ++-
gazebo/common/SphericalCoordinatesPrivate.hh | 33 +-
gazebo/common/SphericalCoordinates_TEST.cc | 73 +++-
gazebo/gazebo.cc | 1 +
gazebo/gui/ApplyWrenchDialog.cc | 1 +
gazebo/gui/EntityMaker.hh | 1 +
gazebo/gui/GLWidget.cc | 2 +
gazebo/gui/InsertModelWidget.cc | 1 +
gazebo/gui/MainWindow.cc | 2 +
gazebo/gui/ModelBuilderWidget.cc | 1 +
gazebo/gui/ModelListWidget.cc | 2 +
gazebo/gui/ModelRightMenu.cc | 3 +-
gazebo/gui/ModelSnap.cc | 3 +-
gazebo/gui/OculusWindow.cc | 1 +
gazebo/gui/QTestFixture.cc | 1 +
gazebo/gui/RenderWidget.cc | 2 +
gazebo/gui/SpaceNav.cc | 1 +
gazebo/gui/TimePanel.cc | 1 +
gazebo/gui/TimeWidget_TEST.cc | 151 ++++++++
gazebo/gui/TimeWidget_TEST.hh | 4 +
gazebo/gui/ToolsWidget.cc | 3 +-
gazebo/gui/building/BuildingEditor.cc | 1 +
gazebo/gui/building/BuildingEditorPalette.cc | 1 +
gazebo/gui/building/BuildingMaker.cc | 2 +
gazebo/gui/building/BuildingModelManip.cc | 1 +
gazebo/gui/building/EditorView.cc | 1 +
gazebo/gui/building/LevelWidget.cc | 1 +
gazebo/gui/building/ScaleWidget.cc | 1 +
gazebo/gui/model/JointMaker.cc | 1 +
gazebo/gui/model/ModelCreator.cc | 1 +
gazebo/gui/model/ModelEditor.cc | 1 +
gazebo/gui/model/ModelEditorPalette.cc | 1 +
gazebo/gui/terrain/TerrainEditorPalette.cc | 1 +
gazebo/gui/viewers/CMakeLists.txt | 1 +
gazebo/gui/viewers/LaserView.cc | 27 +-
gazebo/gui/viewers/LaserView.hh | 3 +
gazebo/gui/viewers/LaserView_TEST.cc | 107 ++++++
.../LaserView_TEST.hh} | 20 +-
gazebo/physics/ContactManager.cc | 3 +-
gazebo/physics/Entity.cc | 2 +
gazebo/physics/Entity.hh | 1 +
gazebo/physics/Gripper.cc | 3 +-
gazebo/physics/Inertial.cc | 1 +
gazebo/physics/Link.cc | 2 +
gazebo/physics/Model.cc | 2 +
gazebo/physics/Model.hh | 1 +
gazebo/physics/World.cc | 8 +-
gazebo/physics/bullet/BulletJoint.cc | 1 +
gazebo/physics/bullet/BulletLink.cc | 10 +
gazebo/physics/bullet/BulletLink.hh | 3 +
gazebo/physics/dart/DARTJoint.cc | 1 +
gazebo/physics/dart/DARTLink.cc | 16 +
gazebo/physics/dart/DARTLink.hh | 3 +
gazebo/physics/dart/DARTScrewJoint.cc | 16 +-
gazebo/physics/dart/DARTTypes.hh | 15 +
gazebo/physics/ode/ODEJoint.cc | 17 +-
gazebo/physics/ode/ODELink.cc | 111 +++---
gazebo/physics/ode/ODELink.hh | 6 +
gazebo/physics/ode/ODEPhysics.cc | 7 +
gazebo/physics/ode/ODEPhysics.hh | 4 +-
gazebo/physics/simbody/SimbodyLink.cc | 6 +
gazebo/physics/simbody/SimbodyLink.hh | 3 +
gazebo/rendering/COMVisual.cc | 52 ++-
gazebo/rendering/COMVisual.hh | 10 +-
gazebo/rendering/Camera.cc | 12 +-
gazebo/rendering/Camera.hh | 1 +
gazebo/rendering/CameraVisual.cc | 2 +
gazebo/rendering/ContactVisual.cc | 3 +-
gazebo/rendering/InertiaVisual.cc | 53 ++-
gazebo/rendering/InertiaVisual.hh | 10 +-
gazebo/rendering/LaserVisual.cc | 2 +
gazebo/rendering/Projector.cc | 2 +
gazebo/rendering/RenderEngine.cc | 9 +-
gazebo/rendering/Road2d.cc | 1 +
gazebo/rendering/Scene.cc | 8 +-
gazebo/rendering/SonarVisual.cc | 3 +-
gazebo/rendering/TransmitterVisual.cc | 3 +-
gazebo/rendering/UserCamera.cc | 1 +
gazebo/rendering/VideoVisual.cc | 1 +
gazebo/rendering/Visual.cc | 19 +-
gazebo/rendering/Visual.hh | 1 +
gazebo/rendering/VisualPrivate.hh | 1 +
gazebo/rendering/WrenchVisual.cc | 3 +-
gazebo/sensors/AltimeterSensor.cc | 1 +
gazebo/sensors/CameraSensor.cc | 3 +
gazebo/sensors/ContactSensor.cc | 1 +
gazebo/sensors/DepthCameraSensor.cc | 1 +
gazebo/sensors/ForceTorqueSensor.cc | 2 +
gazebo/sensors/GpsSensor.cc | 3 +-
gazebo/sensors/GpuRaySensor.cc | 3 +
gazebo/sensors/GpuRaySensor.hh | 1 +
gazebo/sensors/GpuRaySensor_TEST.cc | 1 +
gazebo/sensors/ImuSensor.cc | 1 +
gazebo/sensors/LogicalCameraSensor.cc | 1 +
gazebo/sensors/MagnetometerSensor.cc | 1 +
gazebo/sensors/MultiCameraSensor.cc | 2 +
gazebo/sensors/Noise.cc | 1 +
gazebo/sensors/Noise.hh | 1 +
gazebo/sensors/Noise_TEST.cc | 1 +
gazebo/sensors/RaySensor.cc | 2 +
gazebo/sensors/Sensor.cc | 32 +-
gazebo/sensors/Sensor.hh | 9 +
gazebo/sensors/SensorManager.cc | 1 +
gazebo/sensors/Sensor_TEST.cc | 9 +
gazebo/sensors/SonarSensor.cc | 3 +-
gazebo/sensors/WirelessReceiver_TEST.cc | 2 +-
gazebo/sensors/WirelessTransceiver.cc | 1 +
gazebo/test/helper_physics_generator.hh | 10 +
gazebo/transport/Connection.cc | 2 +
gazebo/transport/ConnectionManager.cc | 3 +-
gazebo/transport/IOManager.cc | 1 +
gazebo/transport/Node.cc | 1 +
gazebo/transport/Node.hh | 1 +
gazebo/transport/Publication.cc | 2 +
gazebo/transport/Publication.hh | 1 +
gazebo/transport/PublicationTransport.cc | 2 +
gazebo/transport/PublicationTransport.hh | 1 +
gazebo/transport/Publisher.cc | 3 +-
gazebo/transport/SubscriptionTransport.cc | 2 +
gazebo/transport/SubscriptionTransport.hh | 1 +
gazebo/transport/TopicManager.cc | 1 +
gazebo/transport/TopicManager.hh | 1 +
gazebo/util/Diagnostics.cc | 2 +
gazebo/util/LogRecord.cc | 2 +
gazebo/util/LogRecord.hh | 1 +
interfaces/player/CameraInterface.cc | 1 +
interfaces/player/LaserInterface.cc | 1 +
plugins/ModelPropShop.hh | 2 +-
plugins/MudPlugin.cc | 1 +
plugins/PressurePlugin.cc | 1 +
plugins/events/CMakeLists.txt | 2 +
plugins/events/JointEventSource.cc | 279 +++++++++++++++
plugins/events/JointEventSource.hh | 135 ++++++++
plugins/events/SimEventsPlugin.cc | 37 +-
plugins/rest_web/RestApi.cc | 4 +-
plugins/rest_web/RestWebPlugin.cc | 8 +-
test/cmake/plugin/CMakeLists.txt | 8 +-
test/examples/CMakeLists.txt | 5 +
test/examples/examples_build.cc | 10 +-
test/integration/CMakeLists.txt | 2 +
test/integration/camera_sensor.cc | 66 ++++
test/integration/dem.cc | 10 +-
test/integration/joint_gearbox.cc | 102 +++---
test/integration/joint_screw.cc | 6 -
test/integration/physics_friction.cc | 12 +-
test/integration/physics_link.cc | 1 +
test/integration/physics_msgs_inertia.cc | 275 +++++++++++++++
test/integration/physics_solver.cc | 120 +++++++
test/integration/sim_events.cc | 56 ++-
test/integration/world_reset.cc | 105 +++++-
test/pkgconfig/plugin/CMakeLists.txt | 13 +-
test/regression/1694_world_accel.cc | 78 +++++
test/regression/1702_remove_model_scene_nodes.cc | 152 ++++++++
test/regression/CMakeLists.txt | 2 +
test/worlds/gearbox.world | 10 +-
test/worlds/issue_1694.world | 73 ++++
tools/gz.cc | 8 +-
tools/gz.hh | 2 +-
tools/gz_TEST.cc | 10 +-
tools/gz_log.cc | 2 +-
worlds/CMakeLists.txt | 1 +
worlds/seesaw.world | 211 +++++++++++
worlds/seesaw.world.erb | 112 ++++++
worlds/sim_events.world | 236 +++++++++++++
214 files changed, 4063 insertions(+), 510 deletions(-)
diff --git a/.hg_archival.txt b/.hg_archival.txt
index e76edd5..3442cfb 100644
--- a/.hg_archival.txt
+++ b/.hg_archival.txt
@@ -1,5 +1,5 @@
repo: e803bb5fe03c1370cbb9f434912cd73fd1e7942d
-node: 6f51fd02c306714d862254456de294b3f41c843c
+node: 0d7e3f2d0b6edcabe4d5d9cb31ea09d250f95326
branch: gazebo6
-latesttag: gazebo6_6.0.0
-latesttagdistance: 30
+latesttag: gazebo6_6.4.0
+latesttagdistance: 15
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 06502fd..41fb751 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
set (GAZEBO_MAJOR_VERSION 6)
-set (GAZEBO_MINOR_VERSION 1)
+set (GAZEBO_MINOR_VERSION 5)
# 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.
@@ -48,6 +48,10 @@ endif()
# Documentation
add_subdirectory(doc)
+# Configure documentation uploader
+configure_file("${CMAKE_SOURCE_DIR}/cmake/upload_doc.sh.in"
+ ${CMAKE_BINARY_DIR}/upload_doc.sh @ONLY)
+
# If we're configuring only to build docs, stop here
if (DOC_ONLY)
message(WARNING "Configuration was done in DOC_ONLY mode. You can build documentation (make doc), but nothing else.")
diff --git a/Changelog.md b/Changelog.md
index 3925a11..bd75f7c 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,11 +1,50 @@
## Gazebo 6.0
+### Gazebo 6.5.0 (2015-10-22)
+
+1. Added ability to convert from spherical coordinates to local coordinates.
+ * [Pull request #1955](https://bitbucket.org/osrf/gazebo/pull-request/1955)
+
+### Gazebo 6.4.0 (2015-10-14)
+
+1. Fix ABI problem. Make `Sensor::SetPose` function non virtual.
+ * [Pull request #1947](https://bitbucket.org/osrf/gazebo/pull-request/1947)
+
+1. Update inertia properties during simulation
+ * [Pull request #1909](https://bitbucket.org/osrf/gazebo/pull-requests/1909)
+ * [Design document](https://bitbucket.org/osrf/gazebo_design/src/default/inertia_resize/inertia_resize.md)
+
+1. Fix transparency correction for opaque materials
+ * [Pull request #1946](https://bitbucket.org/osrf/gazebo/pull-requests/1946/fix-transparency-correction-for-opaque/diff)
+
+### Gazebo 6.3.0 (2015-10-06)
+
+1. Added `Sensor::SetPose` function
+ * [Pull request #1935](https://bitbucket.org/osrf/gazebo/pull-request/1935)
+
+### Gazebo 6.2.0 (2015-10-02)
+
+1. Update physics when the world is reset
+ * Backport of [pull request #1903](https://bitbucket.org/osrf/gazebo/pull-request/1903)
+ * [Pull request #1916](https://bitbucket.org/osrf/gazebo/pull-request/1916)
+ * [Issue #101](https://bitbucket.org/osrf/gazebo/issue/101)
+
+1. Added Copy constructor and assignment operator to MouseEvent
+ * [Pull request #1855](https://bitbucket.org/osrf/gazebo/pull-request/1855)
+
+### Gazebo 6.1.0 (2015-08-02)
+
1. Added logical_camera sensor.
* [Pull request #1845](https://bitbucket.org/osrf/gazebo/pull-request/1845)
1. Added RandomVelocityPlugin, which applies a random velocity to a model's link.
* [Pull request #1839](https://bitbucket.org/osrf/gazebo/pull-request/1839)
+1. Sim events for joint position, velocity and applied force
+ * [Pull request #1849](https://bitbucket.org/osrf/gazebo/pull-request/1849)
+
+### Gazebo 6.0.0 (2015-07-27)
+
1. Added magnetometer sensor. A contribution from Andrew Symington.
* [Pull request #1788](https://bitbucket.org/osrf/gazebo/pull-request/1788)
@@ -364,6 +403,8 @@ compilation on Windows.
### Gazebo 5.x.x
+### Gazebo 5.2.0 (2015-10-02)
+
1. Initialize sigact struct fields that valgrind said were being used uninitialized
* [Pull request #1809](https://bitbucket.org/osrf/gazebo/pull-request/1809)
@@ -958,6 +999,11 @@ compilation on Windows.
1. Fixed memory leak in image conversion
* [Pull request #1073](https://bitbucket.org/osrf/gazebo/pull-request/1073)
+### Gazebo 2.2.1 (xxxx-xx-xx)
+
+1. Fix heightmap model texture loading.
+ * [Pull request #1596](https://bitbucket.org/osrf/gazebo/pull-request/1596)
+
### Gazebo 2.2.0 (2014-01-10)
1. Fix compilation when using OGRE-1.9 (full support is being worked on)
diff --git a/cmake/GazeboTestUtils.cmake b/cmake/GazeboTestUtils.cmake
index 7a44e2a..2cb5861 100644
--- a/cmake/GazeboTestUtils.cmake
+++ b/cmake/GazeboTestUtils.cmake
@@ -5,7 +5,7 @@
macro (gz_build_tests)
# Build all the tests
foreach(GTEST_SOURCE_file ${ARGN})
- string(REGEX REPLACE ".cc" "" BINARY_NAME ${GTEST_SOURCE_file})
+ string(REGEX REPLACE "\\.cc" "" BINARY_NAME ${GTEST_SOURCE_file})
set(BINARY_NAME ${TEST_TYPE}_${BINARY_NAME})
if(USE_LOW_MEMORY_TESTS)
add_definitions(-DUSE_LOW_MEMORY_TESTS=1)
@@ -108,7 +108,15 @@ if (VALID_DISPLAY)
add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME}
-xml -o ${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
- set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240)
+ set(_env_vars)
+ list(APPEND _env_vars "CMAKE_PREFIX_PATH=${CMAKE_BINARY_DIR}:${CMAKE_PREFIX_PATH}")
+ list(APPEND _env_vars "GAZEBO_PLUGIN_PATH=${CMAKE_BINARY_DIR}/plugins:${CMAKE_BINARY_DIR}/plugins/events:${CMAKE_BINARY_DIR}/plugins/rest_web")
+ list(APPEND _env_vars "GAZEBO_RESOURCE_PATH=${CMAKE_SOURCE_DIR}")
+ list(APPEND _env_vars "PATH=${CMAKE_BINARY_DIR}/gazebo:${CMAKE_BINARY_DIR}/tools:$ENV{PATH}")
+ list(APPEND _env_vars "PKG_CONFIG_PATH=${CMAKE_BINARY_DIR}/cmake/pkgconfig:$PKG_CONFIG_PATH")
+ set_tests_properties(${BINARY_NAME} PROPERTIES
+ TIMEOUT 240
+ ENVIRONMENT "${_env_vars}")
# Check that the test produced a result and create a failure if it didn't.
# Guards against crashed and timed out tests.
diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake
index d94bf4b..c17beae 100644
--- a/cmake/SearchForStuff.cmake
+++ b/cmake/SearchForStuff.cmake
@@ -476,7 +476,7 @@ endif ()
########################################
# Find SDFormat
-set (SDFormat_MIN_VERSION 3.1.0)
+set (SDFormat_MIN_VERSION 3.1.1)
find_package(SDFormat ${SDFormat_MIN_VERSION})
if (NOT SDFormat_FOUND)
diff --git a/cmake/gazebo-config.cmake.in b/cmake/gazebo-config.cmake.in
index 48c08f4..3543fde 100644
--- a/cmake/gazebo-config.cmake.in
+++ b/cmake/gazebo-config.cmake.in
@@ -4,7 +4,62 @@ endif()
set(@PKG_NAME at _CONFIG_INCLUDED TRUE)
set(@PKG_NAME at _VERSION @GAZEBO_VERSION@)
-set(@PKG_NAME at _PLUGIN_PATH @CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@/gazebo- at GAZEBO_VERSION@/plugins)
+set(@PKG_NAME at _PLUGIN_PATH "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@/gazebo- at GAZEBO_VERSION@/plugins")
+
+# The media path contains the location on disk where images,
+# materials scripts, shaders, and other related resources are stored.
+set(@PKG_NAME at _MEDIA_PATH "@CMAKE_INSTALL_PREFIX@/share/gazebo- at GAZEBO_VERSION@/media")
+
+#################################################
+# @PKG_NAME at _PROTO_PATH, @PKG_NAME at _PROTO_INCLUDE_DIRS, and
+# @PKG_NAME at _PROTO_LIBRARIES
+#
+# These three variables allow Gazebo messages to be used in other projects.
+#
+# The following examples are for demonstration purposes and are
+# incomplete. The first example shows how to use a Gazebo message in a
+# custom proto file. The second example shows how to run 'protoc' against
+# custom proto files that make use Gazebo message definitions. The third
+# example shows how to include the correct directory when compiling a library
+# or executable that uses your custom messages.
+#
+# 1. Use a Gazebo message in a custom proto file:
+#
+# package my.msgs;
+# import "vector3d.proto";
+#
+# message MyMessage
+# {
+# required gazebo.msgs.Vector3d p = 1;
+# }
+#
+# 2. Run protoc from a CMakeLists.txt to generate your message's
+# header and source files:
+#
+# add_custom_command(
+# OUTPUT
+# "${proto_filename}.pb.cc"
+# "${proto_filename}.pb.h"
+# COMMAND protoc
+# ARGS --proto_path ${GAZEBO_PROTO_PATH} ${proto_file_out}
+# COMMENT "Running C++ protocol buffer compiler on ${proto_filename}"
+# VERBATIM)
+#
+# 3. When compiling your library or executable, make sure to use the following
+# in the CMakeLists.txt file:
+#
+# include_directories(@PKG_NAME at _PROTO_INCLUDE_DIRS)
+# target_link_libraries(your_package @PKG_NAME at _PROTO_LIBRARIES)
+#
+set(@PKG_NAME at _PROTO_PATH
+ "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@/gazebo- at GAZEBO_VERSION@/gazebo/msgs/proto")
+find_library(gazebo_proto_msgs_lib gazebo_msgs
+ PATHS "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@" NO_DEFAULT_PATH)
+list(APPEND @PKG_NAME at _PROTO_LIBRARIES ${gazebo_proto_msgs_lib})
+list(APPEND @PKG_NAME at _PROTO_INCLUDE_DIRS
+ "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@/gazebo- at GAZEBO_VERSION@/gazebo/msgs")
+# End @PKG_NAME at _PROTO_PATH, @PKG_NAME at _PROTO_INCLUDE_DIRS, and
+# @PKG_NAME at _PROTO_LIBRARIES
list(APPEND @PKG_NAME at _INCLUDE_DIRS @CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@)
list(APPEND @PKG_NAME at _INCLUDE_DIRS @CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@/gazebo- at GAZEBO_VERSION@)
@@ -46,7 +101,11 @@ set(CMAKE_MODULE_PATH
foreach(dep @PKG_DEPENDS@)
if(NOT ${dep}_FOUND)
- find_package(${dep} REQUIRED)
+ if (${dep} MATCHES "Boost")
+ find_package(Boost @MIN_BOOST_VERSION@ REQUIRED thread signals system filesystem program_options regex iostreams date_time)
+ else()
+ find_package(${dep} REQUIRED)
+ endif()
endif()
list(APPEND @PKG_NAME at _INCLUDE_DIRS ${${dep}_INCLUDE_DIRS})
diff --git a/cmake/gazebo_config.h.in b/cmake/gazebo_config.h.in
index 33b11df..2102ea7 100644
--- a/cmake/gazebo_config.h.in
+++ b/cmake/gazebo_config.h.in
@@ -32,6 +32,7 @@
#cmakedefine HAVE_SPNAV 1
#cmakedefine HDF5_INSTRUMENT 1
#cmakedefine HAVE_GRAPHVIZ 1
+#cmakedefine HAVE_UUID 1
#ifdef BUILD_TYPE_PROFILE
#include <google/heap-checker.h>
diff --git a/cmake/upload_doc.sh.in b/cmake/upload_doc.sh.in
new file mode 100644
index 0000000..bd82d09
--- /dev/null
+++ b/cmake/upload_doc.sh.in
@@ -0,0 +1,36 @@
+#!/bin/sh
+
+# Check if the node was configured to use s3cmd
+# This is done by running s3cmd --configure
+if [ ! -f "${HOME}/.s3cfg" ]; then
+ echo "No $HOME/.s3cfg file found. Please config the software first in your system"
+ exit 1
+fi
+
+# Make documentation if not build
+make doc
+if [ ! -f "@CMAKE_BINARY_DIR@/doxygen/html/index.html" ]; then
+ echo "Documentation not present. Install doxygen, and run `make doc` in the build directory"
+ exit 1
+fi
+
+# The code API
+s3cmd sync @CMAKE_BINARY_DIR@/doxygen/html/* s3://osrf-distributions/gazebo/api/@GAZEBO_VERSION_FULL@/ --dry-run -v
+
+echo -n "Upload code API(Y/n)? "
+read ans
+
+if [ "$ans" = "y" ] || [ "$ans" = "Y" ]; then
+ s3cmd sync @CMAKE_BINARY_DIR@/doxygen/html/* s3://osrf-distributions/gazebo/api/@GAZEBO_VERSION_FULL@/ -v
+fi
+
+
+# The msgs API
+s3cmd sync @CMAKE_BINARY_DIR@/doxygen_msgs/html/* s3://osrf-distributions/gazebo/api/@GAZEBO_VERSION_FULL@/ --dry-run -v
+
+echo -n "Upload msg API (Y/n)? "
+read ans
+
+if [ "$ans" = "y" ] || [ "$ans" = "Y" ]; then
+ s3cmd sync @CMAKE_BINARY_DIR@/doxygen_msgs/html/* s3://osrf-distributions/gazebo/msg-api/@GAZEBO_VERSION_FULL@/ -v
+fi
diff --git a/deps/opende/CMakeLists.txt b/deps/opende/CMakeLists.txt
index ce15870..6cf149f 100644
--- a/deps/opende/CMakeLists.txt
+++ b/deps/opende/CMakeLists.txt
@@ -24,6 +24,11 @@ if (HAVE_DART)
link_directories(${DARTCore_LIBRARY_DIRS})
endif()
+if (HAVE_BULLET)
+ include_directories(${BULLET_INCLUDE_DIRS})
+ link_directories(${BULLET_LIBRARY_DIRS})
+endif()
+
add_subdirectory(OPCODE)
add_subdirectory(GIMPACT)
add_subdirectory(ou)
@@ -195,6 +200,8 @@ src/robuststep.cpp
src/rotation.cpp
src/sphere.cpp
src/step.cpp
+src/step_bullet_lemke_wrapper.cpp
+src/step_bullet_pgs_wrapper.cpp
src/step_dart_pgs_wrapper.cpp
src/symm.c
src/timer.cpp
diff --git a/deps/opende/include/ode/objects.h b/deps/opende/include/ode/objects.h
index 27f4c7c..ecd031c 100644
--- a/deps/opende/include/ode/objects.h
+++ b/deps/opende/include/ode/objects.h
@@ -44,7 +44,8 @@ enum Friction_Model {
enum World_Solver_Type{
ODE_DEFAULT,
DART_PGS,
- BULLET_PGS
+ BULLET_PGS,
+ BULLET_LEMKE
};
/**
diff --git a/deps/opende/src/joints/gearbox.cpp b/deps/opende/src/joints/gearbox.cpp
index d96a39a..14c736a 100644
--- a/deps/opende/src/joints/gearbox.cpp
+++ b/deps/opende/src/joints/gearbox.cpp
@@ -66,6 +66,9 @@ dxJointGearbox::getInfo2( dxJoint::Info2* info )
dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);
dBodyVectorToWorld(node[1].body, axis2[0], axis2[1], axis2[2], globalAxis2);
+ dJointSetGearboxReferenceBody1(this, refBody1);
+ dJointSetGearboxReferenceBody2(this, refBody2);
+
double ang1 = getHingeAngle(refBody1,node[0].body,globalAxis1,qrel1);
double ang2 = getHingeAngle(refBody2,node[1].body,globalAxis2,qrel2);
// printf("a1(%f) a10(%f) a2(%f) a20(%f)\n",
diff --git a/deps/opende/src/step.cpp b/deps/opende/src/step.cpp
index 8bf379d..e44773b 100644
--- a/deps/opende/src/step.cpp
+++ b/deps/opende/src/step.cpp
@@ -38,6 +38,11 @@
#include "step_dart_pgs_wrapper.h"
#endif
+#ifdef HAVE_BULLET
+#include "step_bullet_lemke_wrapper.h"
+#include "step_bullet_pgs_wrapper.h"
+#endif
+
#ifdef HDF5_INSTRUMENT
#include <ode/h5dump.h>
#endif
@@ -746,6 +751,24 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
dMessage(d_ERR_LCP, "HAVE_DART is NOT defined");
#endif
}
+ else if (solver_type == BULLET_LEMKE)
+ {
+#ifdef HAVE_BULLET
+#ifdef LIBBULLET_VERSION_GT_282
+ dSolveLCP_bullet_lemke(m, A, lambda, rhs, lo, hi);
+#endif
+#else
+ dMessage(d_ERR_LCP, "HAVE_BULLET is NOT defined");
+#endif
+ }
+ else if (solver_type == BULLET_PGS)
+ {
+#ifdef HAVE_BULLET
+ dSolveLCP_bullet_pgs(m, A, lambda, rhs, lo, hi, findex);
+#else
+ dMessage(d_ERR_LCP, "HAVE_DART is NOT defined");
+#endif
+ }
else
{
dMessage(d_ERR_LCP, "Unrecognized Solver Type");
@@ -842,8 +865,21 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
// (over the given timestep)
IFTIMING(dTimerNow ("update position"));
dxBody *const *const bodyend = body + nb;
- for (dxBody *const *bodycurr = body; bodycurr != bodyend; ++bodycurr) {
+ dReal *cforcecurr = cforce;
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend;
+ cforcecurr+=8, ++bodycurr)
+ {
dxBody *b = *bodycurr;
+ {
+ // sum all forces (external and constraint) into facc and tacc
+ // so dBodyGetForce and dBodyGetTorque returns total force and torque
+ // on the body
+ for (unsigned int j = 0; j < 3; ++j)
+ {
+ b->facc[j] += cforcecurr[j];
+ b->tacc[j] += cforcecurr[4+j];
+ }
+ }
dxStepBody (b,stepsize);
}
}
diff --git a/gazebo/gui/TimeWidget_TEST.hh b/deps/opende/src/step_bullet_inc.h
similarity index 53%
copy from gazebo/gui/TimeWidget_TEST.hh
copy to deps/opende/src/step_bullet_inc.h
index 5daf76b..65cca63 100644
--- a/gazebo/gui/TimeWidget_TEST.hh
+++ b/deps/opende/src/step_bullet_inc.h
@@ -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.
@@ -14,22 +14,19 @@
* limitations under the License.
*
*/
+#ifndef _GAZEBO_ODE_STEP_BULLET_INC_H_
+#define _GAZEBO_ODE_STEP_BULLET_INC_H_
-#ifndef _TIMEPANEL_TEST_HH_
-#define _TIMEPANEL_TEST_HH_
-
-#include "gazebo/gui/QTestFixture.hh"
-
-/// \brief A test class for the TimeWidget widget.
-class TimeWidget_TEST : public QTestFixture
-{
- Q_OBJECT
-
- /// \brief Test that the TimeWidget gets valid times from the server.
- private slots: void ValidTimes();
-
- /// \brief Test setting the visibility of TimeWidget child widgets.
- private slots: void Visibility();
-};
-
+#ifdef HAVE_BULLET
+// This disable warning messages
+#pragma GCC system_header
+#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
+#ifdef LIBBULLET_VERSION_GT_282
+#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
+#endif
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#endif
+#endif
+
diff --git a/deps/opende/src/step_bullet_lemke_wrapper.cpp b/deps/opende/src/step_bullet_lemke_wrapper.cpp
new file mode 100644
index 0000000..25f74f5
--- /dev/null
+++ b/deps/opende/src/step_bullet_lemke_wrapper.cpp
@@ -0,0 +1,74 @@
+/*
+ * 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 <ode/odeconfig.h>
+#include <ode/odemath.h>
+#include <ode/rotation.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include <ode/matrix.h>
+#include "config.h"
+#include "objects.h"
+#include "joints/joint.h"
+#include "util.h"
+#include "joints/hinge.h"
+#include "gazebo/gazebo_config.h"
+#include "step_bullet_lemke_wrapper.h"
+
+#ifdef HAVE_BULLET
+#ifdef LIBBULLET_VERSION_GT_282
+#include "step_bullet_inc.h"
+
+//////////////////////////////////////////////////////////
+void dSolveLCP_bullet_lemke(int _m, dReal *_A, dReal *_x, dReal *_b,
+ dReal *_lo, dReal *_hi)
+{
+ int i, j, rindex, cindex;
+ btMatrixXu A(_m, _m);
+ btVectorXu b(_m), x(_m), lo(_m), hi(_m);
+ btAlignedObjectArray<int> limitDependency;
+ // TODO: pass from world file
+ int numIterations = 1000;
+ bool useSparsity = true;
+ rindex = 0;
+ for (i=0; i<_m; ++i)
+ {
+ cindex = 0;
+ for (j=0; j<_m+1; ++j)
+ {
+ if (j<_m)
+ A.setElem(rindex, cindex, _A[i*(_m+1)+j]);
+ if(i==j && dFabs(A(i, j)) < 1e-12)
+ A.setElem(rindex, cindex, 1e-5);
+ cindex++;
+ }
+ rindex++;
+ b[i] = _b[i];
+ x[i] = _x[i];
+ lo[i] = _lo[i];
+ hi[i] = _hi[i];
+ }
+
+ btLemkeSolver* btlemke_solver = new btLemkeSolver();
+ btlemke_solver->solveMLCP(A, b, x, lo, hi,
+ limitDependency, numIterations, useSparsity);
+
+ // convert x to dReal format
+ for(i=0; i<_m; i++)
+ _x[i] = x[i];
+}
+#endif
+#endif
diff --git a/gazebo/gui/TimeWidget_TEST.hh b/deps/opende/src/step_bullet_lemke_wrapper.h
similarity index 54%
copy from gazebo/gui/TimeWidget_TEST.hh
copy to deps/opende/src/step_bullet_lemke_wrapper.h
index 5daf76b..322cb90 100644
--- a/gazebo/gui/TimeWidget_TEST.hh
+++ b/deps/opende/src/step_bullet_lemke_wrapper.h
@@ -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.
@@ -14,22 +14,13 @@
* limitations under the License.
*
*/
+#ifndef _GAZEBO_ODE_STEP_BULLET_LEMKE_WRAPPER_H_
+#define _GAZEBO_ODE_STEP_BULLET_LEMKE_WRAPPER_H_
-#ifndef _TIMEPANEL_TEST_HH_
-#define _TIMEPANEL_TEST_HH_
-
-#include "gazebo/gui/QTestFixture.hh"
-
-/// \brief A test class for the TimeWidget widget.
-class TimeWidget_TEST : public QTestFixture
-{
- Q_OBJECT
-
- /// \brief Test that the TimeWidget gets valid times from the server.
- private slots: void ValidTimes();
-
- /// \brief Test setting the visibility of TimeWidget child widgets.
- private slots: void Visibility();
-};
-
+#ifdef HAVE_BULLET
+#include <ode/common.h>
+void dSolveLCP_bullet_lemke(int _m, dReal *_A, dReal *_x, dReal *_b,
+ dReal *_lo, dReal *_hi);
+#endif
#endif
+
diff --git a/deps/opende/src/step_bullet_pgs_wrapper.cpp b/deps/opende/src/step_bullet_pgs_wrapper.cpp
new file mode 100644
index 0000000..9670ba1
--- /dev/null
+++ b/deps/opende/src/step_bullet_pgs_wrapper.cpp
@@ -0,0 +1,69 @@
+/*
+ * 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 <ode/odeconfig.h>
+#include <ode/odemath.h>
+#include <ode/rotation.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include <ode/matrix.h>
+#include "config.h"
+#include "objects.h"
+#include "joints/joint.h"
+#include "util.h"
+#include "joints/hinge.h"
+
+#include "gazebo/gazebo_config.h"
+#include "step_bullet_pgs_wrapper.h"
+
+#ifdef HAVE_BULLET
+#include "step_bullet_inc.h"
+
+//////////////////////////////////////////////////////////
+void dSolveLCP_bullet_pgs(int _m, dReal *_A, dReal *_x, dReal *_b,
+ dReal *_lo, dReal *_hi, int *findex)
+{
+ int i, j;
+ btMatrixXu A(_m, _m);
+ btVectorXu b(_m), x(_m), lo(_m), hi(_m);
+ btAlignedObjectArray<int> limitDependency;
+ // todo: pass from world file
+ int numIterations = 1000;
+ bool useSparsity = true;
+
+ for(i=0; i<_m; ++i)
+ {
+ for(j=0; j<_m; ++j)
+ {
+ A.setElem(i, j, _A[i*_m+j]);
+ if(i==j && dFabs(_A[i*_m+j]) < 1e-12)
+ A.setElem(i, j, 1e-1);
+ }
+ b[i] = _b[i];
+ x[i] = _x[i];
+ lo[i] = _lo[i];
+ hi[i] = _hi[i];
+ limitDependency.push_back(findex[i]);
+ }
+
+ btSolveProjectedGaussSeidel* btPGS_solver = new btSolveProjectedGaussSeidel();
+ btPGS_solver->solveMLCP(A, b, x, lo, hi, limitDependency, numIterations, useSparsity);
+
+ // convert x to dReal format
+ for(i=0; i<_m; i++)
+ _x[i] = x[i];
+}
+#endif
diff --git a/gazebo/gui/TimeWidget_TEST.hh b/deps/opende/src/step_bullet_pgs_wrapper.h
similarity index 54%
copy from gazebo/gui/TimeWidget_TEST.hh
copy to deps/opende/src/step_bullet_pgs_wrapper.h
index 5daf76b..86c07d6 100644
--- a/gazebo/gui/TimeWidget_TEST.hh
+++ b/deps/opende/src/step_bullet_pgs_wrapper.h
@@ -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.
@@ -14,22 +14,10 @@
* limitations under the License.
*
*/
+#ifndef _GAZEBO_ODE_STEP_BULLET_PGS_WRAPPER_H_
+#define _GAZEBO_ODE_STEP_BULLET_PGS_WRAPPER_H_
-#ifndef _TIMEPANEL_TEST_HH_
-#define _TIMEPANEL_TEST_HH_
-
-#include "gazebo/gui/QTestFixture.hh"
-
-/// \brief A test class for the TimeWidget widget.
-class TimeWidget_TEST : public QTestFixture
-{
- Q_OBJECT
-
- /// \brief Test that the TimeWidget gets valid times from the server.
- private slots: void ValidTimes();
-
- /// \brief Test setting the visibility of TimeWidget child widgets.
- private slots: void Visibility();
-};
-
+#include <ode/common.h>
+void dSolveLCP_bullet_pgs(int _m, dReal *_A, dReal *_x, dReal *_b,
+ dReal *_lo, dReal *_hi, int *findex);
#endif
diff --git a/examples/plugins/CMakeLists.txt b/examples/plugins/CMakeLists.txt
deleted file mode 100644
index e0f13a5..0000000
--- a/examples/plugins/CMakeLists.txt
+++ /dev/null
@@ -1,14 +0,0 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
- string (REPLACE ";" " " GAZEBO_CFLAGS "${GAZEBO_CFLAGS}")
-endif()
-
-include_directories(${GAZEBO_INCLUDE_DIRS})
-link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CFLAGS}")
-
-add_library(world_plugin_tutorial SHARED world_plugin_tutorial.cc)
-target_link_libraries(world_plugin_tutorial ${GAZEBO_libraries})
diff --git a/examples/plugins/animate_joints/CMakeLists.txt b/examples/plugins/animate_joints/CMakeLists.txt
index 8fd7064..ec65655 100644
--- a/examples/plugins/animate_joints/CMakeLists.txt
+++ b/examples/plugins/animate_joints/CMakeLists.txt
@@ -1,16 +1,9 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-find_package(Boost REQUIRED system)
-include_directories(${Boost_INCLUDE_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
-
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
-
-include(FindBoost)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(animate_joints SHARED animate_joints.cc)
-target_link_libraries(animate_joints ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(animate_joints ${GAZEBO_LIBRARIES})
diff --git a/examples/plugins/gui_overlay_plugin_spawn/GUIExampleSpawnWidget.cc b/examples/plugins/gui_overlay_plugin_spawn/GUIExampleSpawnWidget.cc
index 795d924..56dd57d 100644
--- a/examples/plugins/gui_overlay_plugin_spawn/GUIExampleSpawnWidget.cc
+++ b/examples/plugins/gui_overlay_plugin_spawn/GUIExampleSpawnWidget.cc
@@ -79,30 +79,16 @@ GUIExampleSpawnWidget::~GUIExampleSpawnWidget()
/////////////////////////////////////////////////
void GUIExampleSpawnWidget::OnButton()
{
+ msgs::Model model;
+ model.set_name("plugin_unit_sphere_" + std::to_string(this->counter++));
+ msgs::Set(model.mutable_pose(), ignition::math::Pose3d(0, 0, 1.5, 0, 0, 0));
+ const double mass = 1.0;
+ const double radius = 0.5;
+ msgs::AddSphereLink(model, mass, radius);
+
std::ostringstream newModelStr;
- newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
- << "<model name='plugin_unit_sphere_" << this->counter++ << "'>"
- << " <pose>0 0 1.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>"
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << msgs::ModelToSDF(model)->ToString("")
<< "</sdf>";
// Send the model to the gazebo server
diff --git a/examples/plugins/hello_world/CMakeLists.txt b/examples/plugins/hello_world/CMakeLists.txt
index 9a30934..360017e 100644
--- a/examples/plugins/hello_world/CMakeLists.txt
+++ b/examples/plugins/hello_world/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(hello_world SHARED hello_world.cc)
target_link_libraries(hello_world ${GAZEBO_LIBRARIES})
diff --git a/examples/plugins/mainwindow_example/CMakeLists.txt b/examples/plugins/mainwindow_example/CMakeLists.txt
index c48088a..c110524 100644
--- a/examples/plugins/mainwindow_example/CMakeLists.txt
+++ b/examples/plugins/mainwindow_example/CMakeLists.txt
@@ -2,11 +2,10 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package (Qt4)
-find_package(Protobuf REQUIRED)
-
include (${QT_USE_FILE})
add_definitions(${QT_DEFINITIONS})
+find_package(gazebo REQUIRED)
include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
@@ -17,4 +16,4 @@ add_library(main_window_example_widget SHARED ${headers_MOC}
MainWindowExampleWidget.cc)
target_link_libraries(main_window_example_widget ${GAZEBO_LIBRARIES}
${QT_LIBRARIES}
- ${PROTOBUF_LIBRARIES})
+)
diff --git a/examples/plugins/mainwindow_example/MainWindowExampleWidget.hh b/examples/plugins/mainwindow_example/MainWindowExampleWidget.hh
index 428fd61..bd53366 100644
--- a/examples/plugins/mainwindow_example/MainWindowExampleWidget.hh
+++ b/examples/plugins/mainwindow_example/MainWindowExampleWidget.hh
@@ -22,8 +22,10 @@
#include <gazebo/common/Plugin.hh>
#include <gazebo/gui/GuiPlugin.hh>
-#include <gazebo/transport/transport.hh>
-#include <gazebo/gui/gui.hh>
+#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829
+# include <gazebo/transport/transport.hh>
+# include <gazebo/gui/gui.hh>
+#endif
namespace gazebo
{
diff --git a/examples/plugins/model_move/CMakeLists.txt b/examples/plugins/model_move/CMakeLists.txt
index cfc825b..2a0eaad 100644
--- a/examples/plugins/model_move/CMakeLists.txt
+++ b/examples/plugins/model_move/CMakeLists.txt
@@ -4,24 +4,17 @@ project(mode_move)
# Find packages
-find_package(Boost REQUIRED COMPONENTS system)
-include_directories(${Boost_INCLUDE_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
-
find_package(Gazebo REQUIRED)
-if (NOT MSVC)
- set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
-endif()
-
# include appropriate directories
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
# Create libraries and executables
add_executable(path_publisher path_publisher.cc)
-target_link_libraries(path_publisher ${GAZEBO_LIBRARIES} ${PROTOBUF_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(path_publisher ${GAZEBO_LIBRARIES})
add_library(model_move SHARED model_move.cc)
-target_link_libraries(model_move ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${SDF_LIBRARIES} ${PROTOBUF_LIBRARIES})
+target_link_libraries(model_move ${GAZEBO_LIBRARIES})
diff --git a/examples/plugins/model_push/CMakeLists.txt b/examples/plugins/model_push/CMakeLists.txt
index 1f141ac..7bf6e0a 100644
--- a/examples/plugins/model_push/CMakeLists.txt
+++ b/examples/plugins/model_push/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(model_push SHARED model_push.cc)
target_link_libraries(model_push ${GAZEBO_LIBRARIES})
diff --git a/examples/plugins/model_push/model_push.world b/examples/plugins/model_push/model_push.world
index 598bdee..2ed6586 100644
--- a/examples/plugins/model_push/model_push.world
+++ b/examples/plugins/model_push/model_push.world
@@ -31,7 +31,7 @@
</visual>
</link>
- <plugin name="model_push" filename="build/libmodel_push.so"/>
+ <plugin name="model_push" filename="libmodel_push.so"/>
</model>
</world>
</sdf>
diff --git a/examples/plugins/parameters/CMakeLists.txt b/examples/plugins/parameters/CMakeLists.txt
index 7a98997..63d56f9 100644
--- a/examples/plugins/parameters/CMakeLists.txt
+++ b/examples/plugins/parameters/CMakeLists.txt
@@ -1,13 +1,9 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-find_package(Boost REQUIRED system)
-include_directories(${Boost_INCLUDE_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
-
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(parameters SHARED parameters.cc)
-target_link_libraries(parameters ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(parameters ${GAZEBO_LIBRARIES})
diff --git a/examples/plugins/projector/CMakeLists.txt b/examples/plugins/projector/CMakeLists.txt
index f737f37..82037e4 100644
--- a/examples/plugins/projector/CMakeLists.txt
+++ b/examples/plugins/projector/CMakeLists.txt
@@ -1,13 +1,9 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-find_package(Boost REQUIRED system)
-include_directories(${Boost_INCLUDE_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
-
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(projector_plugin SHARED projector.cc)
-target_link_libraries(projector_plugin ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(projector_plugin ${GAZEBO_LIBRARIES})
diff --git a/examples/plugins/system_gui_plugin/CMakeLists.txt b/examples/plugins/system_gui_plugin/CMakeLists.txt
index c16b234..a57fa3e 100644
--- a/examples/plugins/system_gui_plugin/CMakeLists.txt
+++ b/examples/plugins/system_gui_plugin/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package(gazebo REQUIRED)
include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS} )
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(system_gui SHARED system_gui.cc)
target_link_libraries(system_gui ${GAZEBO_LIBRARIES})
diff --git a/examples/stand_alone/actuator/CMakeLists.txt b/examples/stand_alone/actuator/CMakeLists.txt
index 166c3cc..d99ddaf 100644
--- a/examples/stand_alone/actuator/CMakeLists.txt
+++ b/examples/stand_alone/actuator/CMakeLists.txt
@@ -1,28 +1,18 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include(FindBoost)
-
find_package(gazebo REQUIRED)
-find_package(Protobuf REQUIRED)
-find_package(SDFormat REQUIRED)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex program_options)
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
include_directories(
- ${SDFormat_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)
link_directories(
- ${SDFormat_LIBRARY_DIRS}
${GAZEBO_LIBRARY_DIRS}
)
add_executable(actuator main.cc)
target_link_libraries(actuator
- ${SDFormat_LIBRARIES}
${GAZEBO_LIBRARIES}
- ${PROTOBUF_LIBRARIES}
- ${Boost_LIBRARIES}
pthread
)
diff --git a/examples/stand_alone/animated_box/CMakeLists.txt b/examples/stand_alone/animated_box/CMakeLists.txt
index b9c709a..54a4977 100644
--- a/examples/stand_alone/animated_box/CMakeLists.txt
+++ b/examples/stand_alone/animated_box/CMakeLists.txt
@@ -4,14 +4,9 @@ project(animated_box)
# Find packages
-find_package(Boost REQUIRED COMPONENTS system)
-include_directories(${Boost_INCLUDE_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
-
-find_package(Protobuf REQUIRED)
find_package(gazebo REQUIRED)
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
# include appropriate directories
include_directories(${GAZEBO_INCLUDE_DIRS})
@@ -20,11 +15,11 @@ link_directories(${GAZEBO_LIBRARY_DIRS})
# Create libraries and executables
add_library(animated_box SHARED animated_box.cc)
-target_link_libraries(animated_box ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(animated_box ${GAZEBO_LIBRARIES})
add_executable(integrated_main integrated_main.cc)
-target_link_libraries(integrated_main ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
+target_link_libraries(integrated_main ${GAZEBO_LIBRARIES} pthread)
add_executable(independent_listener independent_listener.cc)
-target_link_libraries(independent_listener ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
+target_link_libraries(independent_listener ${GAZEBO_LIBRARIES} pthread)
diff --git a/examples/stand_alone/clone_simulation/CMakeLists.txt b/examples/stand_alone/clone_simulation/CMakeLists.txt
index f03b6f8..5f00763 100644
--- a/examples/stand_alone/clone_simulation/CMakeLists.txt
+++ b/examples/stand_alone/clone_simulation/CMakeLists.txt
@@ -1,14 +1,10 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include(FindBoost)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED thread system filesystem regex)
find_package(gazebo REQUIRED)
-find_package(Protobuf REQUIRED)
-
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_executable(cloner cloner.cc)
-target_link_libraries(cloner ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES})
+target_link_libraries(cloner ${GAZEBO_LIBRARIES})
diff --git a/examples/stand_alone/custom_main/CMakeLists.txt b/examples/stand_alone/custom_main/CMakeLists.txt
index 5104317..5a25ec1 100644
--- a/examples/stand_alone/custom_main/CMakeLists.txt
+++ b/examples/stand_alone/custom_main/CMakeLists.txt
@@ -1,18 +1,14 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include(FindBoost)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)
-
-find_package(Protobuf REQUIRED)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_executable(custom_main custom_main.cc)
-target_link_libraries(custom_main ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
+target_link_libraries(custom_main ${GAZEBO_LIBRARIES} pthread)
add_executable(custom_main_vector custom_main_vector.cc)
-target_link_libraries(custom_main_vector ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
+target_link_libraries(custom_main_vector ${GAZEBO_LIBRARIES} pthread)
diff --git a/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt b/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt
index e8f487f..c99ca11 100644
--- a/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt
+++ b/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt
@@ -1,19 +1,14 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include(FindBoost)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)
-
-find_package(Protobuf REQUIRED)
-
include (FindPkgConfig)
if (PKG_CONFIG_FOUND)
pkg_check_modules(GAZEBO gazebo)
endif()
string (REPLACE ";" " " GAZEBO_CFLAGS "${GAZEBO_CFLAGS}")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CFLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CFLAGS}")
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
add_executable(custom_main custom_main.cc)
-target_link_libraries(custom_main ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
+target_link_libraries(custom_main ${GAZEBO_LIBRARIES})
diff --git a/examples/stand_alone/listener/CMakeLists.txt b/examples/stand_alone/listener/CMakeLists.txt
index e947e6f..95454c9 100644
--- a/examples/stand_alone/listener/CMakeLists.txt
+++ b/examples/stand_alone/listener/CMakeLists.txt
@@ -1,14 +1,10 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include(FindBoost)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)
-
-find_package(Protobuf REQUIRED)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_executable(listener listener.cc)
-target_link_libraries(listener ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
+target_link_libraries(listener ${GAZEBO_LIBRARIES} pthread)
diff --git a/examples/stand_alone/publisher/CMakeLists.txt b/examples/stand_alone/publisher/CMakeLists.txt
index 7b75618..d98dbc5 100644
--- a/examples/stand_alone/publisher/CMakeLists.txt
+++ b/examples/stand_alone/publisher/CMakeLists.txt
@@ -1,14 +1,10 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include(FindBoost)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)
-
find_package(gazebo REQUIRED)
-find_package(Protobuf REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
-add_executable(publisher publisher.cc)
-target_link_libraries(publisher ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
+add_executable(publisher publisher.cc)
+target_link_libraries(publisher ${GAZEBO_LIBRARIES})
diff --git a/examples/stand_alone/publisher/publisher.cc b/examples/stand_alone/publisher/publisher.cc
index 821d7cb..114e9ef 100644
--- a/examples/stand_alone/publisher/publisher.cc
+++ b/examples/stand_alone/publisher/publisher.cc
@@ -46,7 +46,7 @@ int main(int _argc, char **_argv)
gazebo::common::Time::MSleep(100);
// Generate a pose
- gazebo::math::Pose pose(1, 2, 3, 4, 5, 6);
+ ignition::math::Pose3d pose(1, 2, 3, 4, 5, 6);
// Convert to a pose message
gazebo::msgs::Pose msg;
diff --git a/examples/stand_alone/test_fixture/CMakeLists.txt b/examples/stand_alone/test_fixture/CMakeLists.txt
index b370e23..6eb0e0b 100644
--- a/examples/stand_alone/test_fixture/CMakeLists.txt
+++ b/examples/stand_alone/test_fixture/CMakeLists.txt
@@ -2,8 +2,6 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(test_fixture_example)
find_package(gazebo REQUIRED)
-include(FindBoost)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED filesystem thread system)
include_directories(${GAZEBO_INCLUDE_DIRS}
${PROJECT_SOURCE_DIR}/gtest/include
@@ -11,7 +9,7 @@ include_directories(${GAZEBO_INCLUDE_DIRS}
)
link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
# Build gtest
add_library(gtest STATIC gtest/src/gtest-all.cc)
@@ -20,4 +18,4 @@ target_link_libraries(gtest_main gtest)
add_executable(test_example test.cc)
target_link_libraries(test_example gtest gtest_main gazebo_test_fixture
- ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
+ ${GAZEBO_LIBRARIES})
diff --git a/examples/stand_alone/transporter/CMakeLists.txt b/examples/stand_alone/transporter/CMakeLists.txt
index 0366942..e1449c2 100644
--- a/examples/stand_alone/transporter/CMakeLists.txt
+++ b/examples/stand_alone/transporter/CMakeLists.txt
@@ -1,16 +1,13 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include(FindBoost)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system)
-
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_executable(transporter transporter.cc)
-target_link_libraries(transporter ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} )
+target_link_libraries(transporter ${GAZEBO_LIBRARIES})
if (UNIX)
target_link_libraries(transporter pthread)
diff --git a/examples/stand_alone/transporter/transporter.cc b/examples/stand_alone/transporter/transporter.cc
index ece3b68..4036e44 100644
--- a/examples/stand_alone/transporter/transporter.cc
+++ b/examples/stand_alone/transporter/transporter.cc
@@ -15,7 +15,7 @@
*
*/
-#include <gazebo/gazebo.hh>
+#include <gazebo/gazebo_client.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
@@ -23,7 +23,7 @@
int main(int _argc, char **_argv)
{
// Load gazebo
- gazebo::setupClient(_argc, _argv);
+ gazebo::client::setup(_argc, _argv);
// Create our node for communication
gazebo::transport::NodePtr node(new gazebo::transport::Node());
@@ -42,5 +42,5 @@ int main(int _argc, char **_argv)
pub->Publish(msg);
// Make sure to shut everything down.
- gazebo::shutdown();
+ gazebo::client::shutdown();
}
diff --git a/gazebo/Master.cc b/gazebo/Master.cc
index e1e1c35..a694fc5 100644
--- a/gazebo/Master.cc
+++ b/gazebo/Master.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include <google/protobuf/descriptor.h>
#include <set>
#include "gazebo/transport/IOManager.hh"
diff --git a/gazebo/common/CMakeLists.txt b/gazebo/common/CMakeLists.txt
index 4d57017..e683996 100644
--- a/gazebo/common/CMakeLists.txt
+++ b/gazebo/common/CMakeLists.txt
@@ -134,6 +134,7 @@ set (gtest_sources
Material_TEST.cc
Mesh_TEST.cc
MeshManager_TEST.cc
+ MouseEvent_TEST.cc
MovingWindowFilter_TEST.cc
SphericalCoordinates_TEST.cc
SystemPaths_TEST.cc
diff --git a/gazebo/common/Event_TEST.cc b/gazebo/common/Event_TEST.cc
index 090df60..69a9185 100644
--- a/gazebo/common/Event_TEST.cc
+++ b/gazebo/common/Event_TEST.cc
@@ -16,6 +16,7 @@
*/
#include <gtest/gtest.h>
+#include <boost/bind.hpp>
#include <gazebo/common/Time.hh>
#include <gazebo/common/Event.hh>
#include "test/util.hh"
diff --git a/gazebo/common/ModelDatabase.cc b/gazebo/common/ModelDatabase.cc
index 80da4d7..dd680c2 100644
--- a/gazebo/common/ModelDatabase.cc
+++ b/gazebo/common/ModelDatabase.cc
@@ -25,7 +25,9 @@
#include <iostream>
#include <boost/algorithm/string/replace.hpp>
+#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
+#include <boost/function.hpp>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>
diff --git a/gazebo/common/ModelDatabase.hh b/gazebo/common/ModelDatabase.hh
index 9bf6edd..284b0e4 100644
--- a/gazebo/common/ModelDatabase.hh
+++ b/gazebo/common/ModelDatabase.hh
@@ -21,6 +21,7 @@
#include <map>
#include <utility>
+#include <boost/function.hpp>
#include "gazebo/common/Event.hh"
#include "gazebo/common/SingletonT.hh"
#include "gazebo/common/CommonTypes.hh"
diff --git a/gazebo/common/ModelDatabasePrivate.hh b/gazebo/common/ModelDatabasePrivate.hh
index edcb343..ef26ae3 100644
--- a/gazebo/common/ModelDatabasePrivate.hh
+++ b/gazebo/common/ModelDatabasePrivate.hh
@@ -21,6 +21,7 @@
#include <map>
#include <string>
+#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
diff --git a/gazebo/common/MouseEvent.cc b/gazebo/common/MouseEvent.cc
index 38cfa63..2e4fd95 100644
--- a/gazebo/common/MouseEvent.cc
+++ b/gazebo/common/MouseEvent.cc
@@ -28,6 +28,13 @@ MouseEvent::MouseEvent()
}
/////////////////////////////////////////////////
+MouseEvent::MouseEvent(const MouseEvent &_other)
+ : dataPtr(new MouseEventPrivate)
+{
+ *dataPtr = *_other.dataPtr;
+}
+
+/////////////////////////////////////////////////
MouseEvent::~MouseEvent()
{
delete this->dataPtr;
@@ -201,3 +208,14 @@ void MouseEvent::SetControl(const bool _control) const
{
this->dataPtr->control = _control;
}
+
+/////////////////////////////////////////////////
+MouseEvent &MouseEvent::operator=(const MouseEvent &_other)
+{
+ if (this == &_other)
+ return *this;
+
+ *this->dataPtr = *_other.dataPtr;
+
+ return *this;
+}
diff --git a/gazebo/common/MouseEvent.hh b/gazebo/common/MouseEvent.hh
index 3c1e5df..82d85ef 100644
--- a/gazebo/common/MouseEvent.hh
+++ b/gazebo/common/MouseEvent.hh
@@ -72,6 +72,10 @@ namespace gazebo
/// \brief Constructor.
public: MouseEvent();
+ /// \brief Copy constructor.
+ /// \param[in] _other Other mouse event
+ public: MouseEvent(const MouseEvent &_other);
+
/// \brief Destructor
public: virtual ~MouseEvent();
@@ -194,6 +198,11 @@ namespace gazebo
/// \param[in] _control The control key flag.
public: void SetControl(const bool _control) const;
+ /// \brief Assignment operator
+ /// \param[in] _other Other mouse event
+ /// \return this
+ public: MouseEvent &operator=(const MouseEvent &_other);
+
/// \internal
/// \brief Private data pointer
private: MouseEventPrivate *dataPtr;
diff --git a/gazebo/common/MouseEvent_TEST.cc b/gazebo/common/MouseEvent_TEST.cc
new file mode 100644
index 0000000..fd85cac
--- /dev/null
+++ b/gazebo/common/MouseEvent_TEST.cc
@@ -0,0 +1,278 @@
+/*
+ * 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 "test_config.h"
+#include "gazebo/common/MouseEvent.hh"
+#include "gazebo/gazebo_config.h"
+#include "test/util.hh"
+
+using namespace gazebo;
+
+class MouseEvent : public gazebo::testing::AutoLogFixture { };
+
+/////////////////////////////////////////////////
+TEST_F(MouseEvent, CopyConstructor)
+{
+ ignition::math::Vector2i pos(1, 25);
+ ignition::math::Vector2i prevPos(2, 26);
+ ignition::math::Vector2i pressPos(3, 24);
+ ignition::math::Vector2i scroll(2, 3);
+ float moveScale = 0.3;
+ bool dragging = true;
+ common::MouseEvent::EventType type = common::MouseEvent::PRESS;
+ common::MouseEvent::MouseButton button = common::MouseEvent::LEFT;
+ bool shift = false;
+ bool alt = false;
+ bool control = false;
+
+ // create mouse event
+ common::MouseEvent event;
+ event.SetPos(pos);
+ event.SetPrevPos(prevPos);
+ event.SetPressPos(pressPos);
+ event.SetScroll(scroll);
+ event.SetMoveScale(moveScale);
+ event.SetDragging(dragging);
+ event.SetType(type);
+ event.SetButton(button);
+ event.SetShift(shift);
+ event.SetAlt(alt);
+ event.SetControl(control);
+
+ // verify mouse event values
+ EXPECT_EQ(event.Pos(), pos);
+ EXPECT_EQ(event.PrevPos(), prevPos);
+ EXPECT_EQ(event.PressPos(), pressPos);
+ EXPECT_EQ(event.Scroll(), scroll);
+ EXPECT_DOUBLE_EQ(event.MoveScale(), moveScale);
+ EXPECT_EQ(event.Dragging(), dragging);
+ EXPECT_EQ(event.Type(), type);
+ EXPECT_EQ(event.Button(), button);
+ EXPECT_EQ(event.Shift(), shift);
+ EXPECT_EQ(event.Alt(), alt);
+ EXPECT_EQ(event.Control(), control);
+
+ // verify copy constructor works
+ common::MouseEvent otherEvent = event;
+ EXPECT_EQ(otherEvent.Pos(), pos);
+ EXPECT_EQ(otherEvent.PrevPos(), prevPos);
+ EXPECT_EQ(otherEvent.PressPos(), pressPos);
+ EXPECT_EQ(otherEvent.Scroll(), scroll);
+ EXPECT_DOUBLE_EQ(otherEvent.MoveScale(), moveScale);
+ EXPECT_EQ(otherEvent.Dragging(), dragging);
+ EXPECT_EQ(otherEvent.Type(), type);
+ EXPECT_EQ(otherEvent.Button(), button);
+ EXPECT_EQ(otherEvent.Shift(), shift);
+ EXPECT_EQ(otherEvent.Alt(), alt);
+ EXPECT_EQ(otherEvent.Control(), control);
+
+ common::MouseEvent otherEvent2(event);
+ EXPECT_EQ(otherEvent2.Pos(), pos);
+ EXPECT_EQ(otherEvent2.PrevPos(), prevPos);
+ EXPECT_EQ(otherEvent2.PressPos(), pressPos);
+ EXPECT_EQ(otherEvent2.Scroll(), scroll);
+ EXPECT_DOUBLE_EQ(otherEvent2.MoveScale(), moveScale);
+ EXPECT_EQ(otherEvent2.Dragging(), dragging);
+ EXPECT_EQ(otherEvent2.Type(), type);
+ EXPECT_EQ(otherEvent2.Button(), button);
+ EXPECT_EQ(otherEvent2.Shift(), shift);
+ EXPECT_EQ(otherEvent2.Alt(), alt);
+ EXPECT_EQ(otherEvent2.Control(), control);
+
+ // update the original mouse event values
+ ignition::math::Vector2i newPos(3, 18);
+ ignition::math::Vector2i newPrevPos(2, 17);
+ ignition::math::Vector2i newPressPos(3, 16);
+ ignition::math::Vector2i newScroll(1, 2);
+ float newMoveScale = 0.4;
+ bool newDragging = false;
+ common::MouseEvent::EventType newType = common::MouseEvent::RELEASE;
+ common::MouseEvent::MouseButton newButton = common::MouseEvent::LEFT;
+ common::MouseEvent::MouseButton newButton2 = common::MouseEvent::RIGHT;
+ bool newShift = true;
+ bool newAlt = false;
+ bool newControl = false;
+
+ event.SetPos(newPos.X(), newPos.Y());
+ event.SetPrevPos(newPrevPos.X(), newPrevPos.Y());
+ event.SetPressPos(newPressPos.X(), newPressPos.Y());
+ event.SetScroll(newScroll.X(), newScroll.Y());
+ event.SetMoveScale(newMoveScale);
+ event.SetDragging(newDragging);
+ event.SetType(newType);
+ event.SetButtons(newButton | newButton2);
+ event.SetShift(newShift);
+ event.SetAlt(newAlt);
+ event.SetControl(newControl);
+
+ // verify new mouse event values
+ EXPECT_EQ(event.Pos(), newPos);
+ EXPECT_EQ(event.PrevPos(), newPrevPos);
+ EXPECT_EQ(event.PressPos(), newPressPos);
+ EXPECT_EQ(event.Scroll(), newScroll);
+ EXPECT_DOUBLE_EQ(event.MoveScale(), newMoveScale);
+ EXPECT_EQ(event.Dragging(), newDragging);
+ EXPECT_EQ(event.Type(), newType);
+ EXPECT_EQ(event.Buttons(), static_cast<unsigned int>(newButton | newButton2));
+ EXPECT_EQ(event.Shift(), newShift);
+ EXPECT_EQ(event.Alt(), newAlt);
+ EXPECT_EQ(event.Control(), newControl);
+
+ // verify copies have the original values
+ EXPECT_EQ(otherEvent.Pos(), pos);
+ EXPECT_EQ(otherEvent.PrevPos(), prevPos);
+ EXPECT_EQ(otherEvent.PressPos(), pressPos);
+ EXPECT_EQ(otherEvent.Scroll(), scroll);
+ EXPECT_DOUBLE_EQ(otherEvent.MoveScale(), moveScale);
+ EXPECT_EQ(otherEvent.Dragging(), dragging);
+ EXPECT_EQ(otherEvent.Type(), type);
+ EXPECT_EQ(otherEvent.Button(), button);
+ EXPECT_EQ(otherEvent.Shift(), shift);
+ EXPECT_EQ(otherEvent.Alt(), alt);
+ EXPECT_EQ(otherEvent.Control(), control);
+
+ EXPECT_EQ(otherEvent2.Pos(), pos);
+ EXPECT_EQ(otherEvent2.PrevPos(), prevPos);
+ EXPECT_EQ(otherEvent2.PressPos(), pressPos);
+ EXPECT_EQ(otherEvent2.Scroll(), scroll);
+ EXPECT_DOUBLE_EQ(otherEvent2.MoveScale(), moveScale);
+ EXPECT_EQ(otherEvent2.Dragging(), dragging);
+ EXPECT_EQ(otherEvent2.Type(), type);
+ EXPECT_EQ(otherEvent2.Button(), button);
+ EXPECT_EQ(otherEvent2.Shift(), shift);
+ EXPECT_EQ(otherEvent2.Alt(), alt);
+ EXPECT_EQ(otherEvent2.Control(), control);
+}
+
+/////////////////////////////////////////////////
+TEST_F(MouseEvent, Assignment)
+{
+ ignition::math::Vector2i pos(1, 25);
+ ignition::math::Vector2i prevPos(2, 26);
+ ignition::math::Vector2i pressPos(3, 24);
+ ignition::math::Vector2i scroll(2, 3);
+ float moveScale = 0.3;
+ bool dragging = true;
+ common::MouseEvent::EventType type = common::MouseEvent::PRESS;
+ common::MouseEvent::MouseButton button = common::MouseEvent::LEFT;
+ bool shift = false;
+ bool alt = false;
+ bool control = false;
+
+ // create mouse event
+ common::MouseEvent event;
+ event.SetPos(pos);
+ event.SetPrevPos(prevPos);
+ event.SetPressPos(pressPos);
+ event.SetScroll(scroll);
+ event.SetMoveScale(moveScale);
+ event.SetDragging(dragging);
+ event.SetType(type);
+ event.SetButton(button);
+ event.SetShift(shift);
+ event.SetAlt(alt);
+ event.SetControl(control);
+
+ // verify mouse event values
+ EXPECT_EQ(event.Pos(), pos);
+ EXPECT_EQ(event.PrevPos(), prevPos);
+ EXPECT_EQ(event.PressPos(), pressPos);
+ EXPECT_EQ(event.Scroll(), scroll);
+ EXPECT_DOUBLE_EQ(event.MoveScale(), moveScale);
+ EXPECT_EQ(event.Dragging(), dragging);
+ EXPECT_EQ(event.Type(), type);
+ EXPECT_EQ(event.Button(), button);
+ EXPECT_EQ(event.Shift(), shift);
+ EXPECT_EQ(event.Alt(), alt);
+ EXPECT_EQ(event.Control(), control);
+
+ // verify asignment operator works
+ common::MouseEvent otherEvent;
+ otherEvent = event;
+ EXPECT_EQ(otherEvent.Pos(), pos);
+ EXPECT_EQ(otherEvent.PrevPos(), prevPos);
+ EXPECT_EQ(otherEvent.PressPos(), pressPos);
+ EXPECT_EQ(otherEvent.Scroll(), scroll);
+ EXPECT_DOUBLE_EQ(otherEvent.MoveScale(), moveScale);
+ EXPECT_EQ(otherEvent.Dragging(), dragging);
+ EXPECT_EQ(otherEvent.Type(), type);
+ EXPECT_EQ(otherEvent.Button(), button);
+ EXPECT_EQ(otherEvent.Shift(), shift);
+ EXPECT_EQ(otherEvent.Alt(), alt);
+ EXPECT_EQ(otherEvent.Control(), control);
+
+ // update the original mouse event values
+ ignition::math::Vector2i newPos(3, 18);
+ ignition::math::Vector2i newPrevPos(2, 17);
+ ignition::math::Vector2i newPressPos(3, 16);
+ ignition::math::Vector2i newScroll(1, 2);
+ float newMoveScale = 0.4;
+ bool newDragging = false;
+ common::MouseEvent::EventType newType = common::MouseEvent::RELEASE;
+ common::MouseEvent::MouseButton newButton = common::MouseEvent::LEFT;
+ common::MouseEvent::MouseButton newButton2 = common::MouseEvent::RIGHT;
+ bool newShift = false;
+ bool newAlt = true;
+ bool newControl = false;
+
+ event.SetPos(newPos.X(), newPos.Y());
+ event.SetPrevPos(newPrevPos.X(), newPrevPos.Y());
+ event.SetPressPos(newPressPos.X(), newPressPos.Y());
+ event.SetScroll(newScroll.X(), newScroll.Y());
+ event.SetMoveScale(newMoveScale);
+ event.SetDragging(newDragging);
+ event.SetType(newType);
+ event.SetButtons(newButton | newButton2);
+ event.SetShift(newShift);
+ event.SetAlt(newAlt);
+ event.SetControl(newControl);
+
+ // verify new mouse event values
+ EXPECT_EQ(event.Pos(), newPos);
+ EXPECT_EQ(event.PrevPos(), newPrevPos);
+ EXPECT_EQ(event.PressPos(), newPressPos);
+ EXPECT_EQ(event.Scroll(), newScroll);
+ EXPECT_DOUBLE_EQ(event.MoveScale(), newMoveScale);
+ EXPECT_EQ(event.Dragging(), newDragging);
+ EXPECT_EQ(event.Type(), newType);
+ EXPECT_EQ(event.Buttons(), static_cast<unsigned int>(newButton | newButton2));
+ EXPECT_EQ(event.Shift(), newShift);
+ EXPECT_EQ(event.Alt(), newAlt);
+ EXPECT_EQ(event.Control(), newControl);
+
+ // verify the copy has the original values
+ EXPECT_EQ(otherEvent.Pos(), pos);
+ EXPECT_EQ(otherEvent.PrevPos(), prevPos);
+ EXPECT_EQ(otherEvent.PressPos(), pressPos);
+ EXPECT_EQ(otherEvent.Scroll(), scroll);
+ EXPECT_DOUBLE_EQ(otherEvent.MoveScale(), moveScale);
+ EXPECT_EQ(otherEvent.Dragging(), dragging);
+ EXPECT_EQ(otherEvent.Type(), type);
+ EXPECT_EQ(otherEvent.Button(), button);
+ EXPECT_EQ(otherEvent.Shift(), shift);
+ EXPECT_EQ(otherEvent.Alt(), alt);
+ EXPECT_EQ(otherEvent.Control(), control);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/common/SphericalCoordinates.cc b/gazebo/common/SphericalCoordinates.cc
index d1dd9ca..f183c54 100644
--- a/gazebo/common/SphericalCoordinates.cc
+++ b/gazebo/common/SphericalCoordinates.cc
@@ -25,15 +25,19 @@ using namespace gazebo;
using namespace common;
// Parameters for EARTH_WGS84 model
-// a: Semi-major equatorial axis (meters)
-// b: Semi-minor polar axis (meters)
-// if: inverse flattening (no units)
// wikipedia: World_Geodetic_System#A_new_World_Geodetic_System:_WGS_84
+
+// a: Equatorial radius. Semi-major axis of the WGS84 spheroid (meters).
const double g_EarthWGS84AxisEquatorial = 6378137.0;
+
+// b: Polar radius. Semi-minor axis of the wgs84 spheroid (meters).
const double g_EarthWGS84AxisPolar = 6356752.314245;
-const double g_EarthSphere = 6371000.0;
-// const double g_EarthWGS84Flattening = 1/298.257223563;
+// if: WGS84 inverse flattening parameter (no units)
+const double g_EarthWGS84Flattening = 1.0/298.257223563;
+
+// Radius of the Earth (meters).
+const double g_EarthRadius = 6371000.0;
//////////////////////////////////////////////////
SphericalCoordinates::SurfaceType SphericalCoordinates::Convert(
@@ -71,11 +75,17 @@ SphericalCoordinates::SphericalCoordinates(const SurfaceType _type,
const math::Angle &_heading)
: dataPtr(new SphericalCoordinatesPrivate)
{
+ // Set the reference and calculate ellipse parameters
this->SetSurfaceType(_type);
- this->SetLatitudeReference(ignition::math::Angle(_latitude.Radian()));
- this->SetLongitudeReference(ignition::math::Angle(_longitude.Radian()));
- this->SetElevationReference(_elevation);
- this->SetHeadingOffset(ignition::math::Angle(_heading.Radian()));
+
+ // Set the coordinate transform parameters
+ this->dataPtr->latitudeReference = _latitude.Radian();
+ this->dataPtr->longitudeReference = _longitude.Radian();
+ this->dataPtr->elevationReference = _elevation;
+ this->dataPtr->headingOffset = _heading.Radian();
+
+ // Generate transformation matrix
+ this->UpdateTransformationMatrix();
}
//////////////////////////////////////////////////
@@ -86,11 +96,17 @@ SphericalCoordinates::SphericalCoordinates(const SurfaceType _type,
const ignition::math::Angle &_heading)
: dataPtr(new SphericalCoordinatesPrivate)
{
+ // Set the reference and calculate ellipse parameters
this->SetSurfaceType(_type);
- this->SetLatitudeReference(_latitude);
- this->SetLongitudeReference(_longitude);
- this->SetElevationReference(_elevation);
- this->SetHeadingOffset(_heading);
+
+ // Set the coordinate transform parameters
+ this->dataPtr->latitudeReference = _latitude;
+ this->dataPtr->longitudeReference = _longitude;
+ this->dataPtr->elevationReference = _elevation;
+ this->dataPtr->headingOffset = _heading;
+
+ // Generate transformation matrix
+ this->UpdateTransformationMatrix();
}
//////////////////////////////////////////////////
@@ -152,6 +168,37 @@ ignition::math::Angle SphericalCoordinates::HeadingOffset() const
void SphericalCoordinates::SetSurfaceType(const SurfaceType &_type)
{
this->dataPtr->surfaceType = _type;
+
+ switch (this->dataPtr->surfaceType)
+ {
+ case EARTH_WGS84:
+ {
+ // Set the semi-major axis
+ this->dataPtr->ellA = g_EarthWGS84AxisEquatorial;
+
+ // Set the semi-minor axis
+ this->dataPtr->ellB = g_EarthWGS84AxisPolar;
+
+ // Set the flattening parameter
+ this->dataPtr->ellF = g_EarthWGS84Flattening;
+
+ // Set the first eccentricity ellipse parameter
+ // https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses
+ this->dataPtr->ellE = sqrt(1.0 -
+ std::pow(this->dataPtr->ellB, 2) / std::pow(this->dataPtr->ellA, 2));
+
+ // Set the second eccentricity ellipse parameter
+ // https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses
+ this->dataPtr->ellP = sqrt(
+ std::pow(this->dataPtr->ellA, 2) / std::pow(this->dataPtr->ellB, 2) -
+ 1.0);
+
+ break;
+ }
+ default:
+ gzerr << "Unknown surface type[" << this->dataPtr->surfaceType << "]\n";
+ break;
+ }
}
//////////////////////////////////////////////////
@@ -165,7 +212,8 @@ void SphericalCoordinates::SetLatitudeReference(
void SphericalCoordinates::SetLatitudeReference(
const ignition::math::Angle &_angle)
{
- this->dataPtr->latitudeReference.Radian(_angle.Radian());
+ this->dataPtr->latitudeReference = _angle;
+ this->UpdateTransformationMatrix();
}
//////////////////////////////////////////////////
@@ -179,25 +227,28 @@ void SphericalCoordinates::SetLongitudeReference(
void SphericalCoordinates::SetLongitudeReference(
const ignition::math::Angle &_angle)
{
- this->dataPtr->longitudeReference.Radian(_angle.Radian());
+ this->dataPtr->longitudeReference = _angle;
+ this->UpdateTransformationMatrix();
}
//////////////////////////////////////////////////
void SphericalCoordinates::SetElevationReference(double _elevation)
{
this->dataPtr->elevationReference = _elevation;
+ this->UpdateTransformationMatrix();
}
//////////////////////////////////////////////////
void SphericalCoordinates::SetHeadingOffset(const math::Angle &_angle)
{
- this->SetHeadingOffset(ignition::math::Angle(_angle.Radian()));
+ this->SetHeadingOffset(_angle.Ign());
}
//////////////////////////////////////////////////
void SphericalCoordinates::SetHeadingOffset(const ignition::math::Angle &_angle)
{
this->dataPtr->headingOffset.Radian(_angle.Radian());
+ this->UpdateTransformationMatrix();
}
//////////////////////////////////////////////////
@@ -208,69 +259,45 @@ math::Vector3 SphericalCoordinates::SphericalFromLocal(
}
//////////////////////////////////////////////////
+math::Vector3 SphericalCoordinates::GlobalFromLocal(
+ const math::Vector3 &_xyz) const
+{
+ return this->GlobalFromLocal(_xyz.Ign());
+}
+
+//////////////////////////////////////////////////
ignition::math::Vector3d SphericalCoordinates::SphericalFromLocal(
const ignition::math::Vector3d &_xyz) const
{
- double radiusMeridional = 1.0;
- double radiusNormal = 1.0;
- double headingSine = sin(this->dataPtr->headingOffset.Radian());
- double headingCosine = cos(this->dataPtr->headingOffset.Radian());
-
- switch (this->dataPtr->surfaceType)
- {
- case EARTH_WGS84:
- // Currently uses radius of curvature equations from wikipedia
- // http://en.wikipedia.org/wiki/Earth_radius#Radius_of_curvature
- {
- double a = g_EarthWGS84AxisEquatorial;
- double b = g_EarthWGS84AxisPolar;
- double ab = a*b;
- double cosLat = cos(this->dataPtr->latitudeReference.Radian());
- double sinLat = sin(this->dataPtr->latitudeReference.Radian());
- double denom = (a*cosLat)*(a*cosLat) + (b*sinLat)*(b*sinLat);
- radiusMeridional = ab*ab / denom / sqrt(denom);
- radiusNormal = a*a / sqrt(denom);
- }
- break;
-
- default:
- break;
- }
-
- ignition::math::Vector3d spherical;
- double east = _xyz.X() * headingCosine - _xyz.Y() * headingSine;
- double north = _xyz.X() * headingSine + _xyz.Y() * headingCosine;
- // Assumes small changes in latitude / longitude.
- // May not work well near the north / south poles.
- ignition::math::Angle deltaLatitude(north / radiusMeridional);
- ignition::math::Angle deltaLongitude(east / radiusNormal);
- // geodetic latitude in degrees
- spherical.X(this->dataPtr->latitudeReference.Degree() +
- deltaLatitude.Degree());
- // geodetic longitude in degrees
- spherical.Y(this->dataPtr->longitudeReference.Degree() +
- deltaLongitude.Degree());
- // altitude relative to sea level
- spherical.Z(this->dataPtr->elevationReference + _xyz.Z());
- return spherical;
+ ignition::math::Vector3d result =
+ this->PositionTransform(_xyz, LOCAL, SPHERICAL);
+ result.X(IGN_RTOD(result.X()));
+ result.Y(IGN_RTOD(result.Y()));
+ return result;
}
//////////////////////////////////////////////////
-math::Vector3 SphericalCoordinates::GlobalFromLocal(
- const math::Vector3 &_xyz) const
+ignition::math::Vector3d SphericalCoordinates::LocalFromSpherical(
+ const ignition::math::Vector3d &_xyz) const
{
- return this->GlobalFromLocal(_xyz.Ign());
+ ignition::math::Vector3d result = _xyz;
+ result.X(IGN_DTOR(result.X()));
+ result.Y(IGN_DTOR(result.Y()));
+ return this->PositionTransform(result, SPHERICAL, LOCAL);
}
//////////////////////////////////////////////////
ignition::math::Vector3d SphericalCoordinates::GlobalFromLocal(
const ignition::math::Vector3d &_xyz) const
{
- double headingSine = sin(this->dataPtr->headingOffset.Radian());
- double headingCosine = cos(this->dataPtr->headingOffset.Radian());
- double east = _xyz.X() * headingCosine - _xyz.Y() * headingSine;
- double north = _xyz.X() * headingSine + _xyz.Y() * headingCosine;
- return ignition::math::Vector3d(east, north, _xyz.Z());
+ return this->VelocityTransform(_xyz, LOCAL, GLOBAL);
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d SphericalCoordinates::LocalFromGlobal(
+ const ignition::math::Vector3d &_xyz) const
+{
+ return this->VelocityTransform(_xyz, GLOBAL, LOCAL);
}
//////////////////////////////////////////////////
@@ -295,10 +322,233 @@ double SphericalCoordinates::Distance(const ignition::math::Angle &_latA,
{
ignition::math::Angle dLat = _latB - _latA;
ignition::math::Angle dLon = _lonB - _lonA;
+
double a = sin(dLat.Radian() / 2) * sin(dLat.Radian() / 2) +
sin(dLon.Radian() / 2) * sin(dLon.Radian() / 2) *
cos(_latA.Radian()) * cos(_latB.Radian());
+
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
- double d = g_EarthSphere * c;
+ double d = g_EarthRadius * c;
return d;
}
+
+//////////////////////////////////////////////////
+void SphericalCoordinates::UpdateTransformationMatrix()
+{
+ // Cache trig results
+ double cosLat = cos(this->dataPtr->latitudeReference.Radian());
+ double sinLat = sin(this->dataPtr->latitudeReference.Radian());
+ double cosLon = cos(this->dataPtr->longitudeReference.Radian());
+ double sinLon = sin(this->dataPtr->longitudeReference.Radian());
+
+ // Create a rotation matrix that moves ECEF to GLOBAL
+ // http://www.navipedia.net/index.php/
+ // Transformations_between_ECEF_and_ENU_coordinates
+ this->dataPtr->rotECEFToGlobal = ignition::math::Matrix3d(
+ -sinLon, cosLon, 0.0,
+ -cosLon * sinLat, -sinLon * sinLat, cosLat,
+ cosLon * cosLat, sinLon * cosLat, sinLat);
+
+ // Create a rotation matrix that moves GLOBAL to ECEF
+ // http://www.navipedia.net/index.php/
+ // Transformations_between_ECEF_and_ENU_coordinates
+ this->dataPtr->rotGlobalToECEF = ignition::math::Matrix3d(
+ -sinLon, -cosLon * sinLat, cosLon * cosLat,
+ cosLon, -sinLon * sinLat, sinLon * cosLat,
+ 0, cosLat, sinLat);
+
+ // Cache heading transforms -- note that we have to negate the heading in
+ // order to preserve backward compatibility. ie. Gazebo has traditionally
+ // expressed positive angle as a CLOCKWISE rotation that takes the GLOBAL
+ // frame to the LOCAL frame. However, right hand coordinate systems require
+ // this to be expressed as an ANTI-CLOCKWISE rotation. So, we negate it.
+ this->dataPtr->cosHea = cos(-this->dataPtr->headingOffset.Radian());
+ this->dataPtr->sinHea = sin(-this->dataPtr->headingOffset.Radian());
+
+ // Cache the ECEF coordinate of the origin
+ this->dataPtr->origin = ignition::math::Vector3d(
+ this->dataPtr->latitudeReference.Radian(),
+ this->dataPtr->longitudeReference.Radian(),
+ this->dataPtr->elevationReference);
+ this->dataPtr->origin =
+ this->PositionTransform(this->dataPtr->origin, SPHERICAL, ECEF);
+}
+
+/////////////////////////////////////////////////
+ignition::math::Vector3d SphericalCoordinates::PositionTransform(
+ const ignition::math::Vector3d &_pos,
+ const CoordinateType &_in, const CoordinateType &_out) const
+{
+ ignition::math::Vector3d tmp = _pos;
+
+ // Cache trig results
+ double cosLat = cos(_pos.X());
+ double sinLat = sin(_pos.X());
+ double cosLon = cos(_pos.Y());
+ double sinLon = sin(_pos.Y());
+
+ // Radius of planet curvature (meters)
+ double curvature = 1.0 -
+ this->dataPtr->ellE * this->dataPtr->ellE * sinLat * sinLat;
+ curvature = this->dataPtr->ellA / sqrt(curvature);
+
+ // Convert whatever arrives to a more flexible ECEF coordinate
+ switch (_in)
+ {
+ // East, North, Up (ENU), note no break at end of case
+ case LOCAL:
+ {
+ tmp.X(-_pos.X() * this->dataPtr->cosHea + _pos.Y() *
+ this->dataPtr->sinHea);
+ tmp.Y(-_pos.X() * this->dataPtr->sinHea - _pos.Y() *
+ this->dataPtr->cosHea);
+ }
+
+ case GLOBAL:
+ {
+ tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp;
+ break;
+ }
+
+ case SPHERICAL:
+ {
+ tmp.X((_pos.Z() + curvature) * cosLat * cosLon);
+ tmp.Y((_pos.Z() + curvature) * cosLat * sinLon);
+ tmp.Z(((this->dataPtr->ellB * this->dataPtr->ellB)/
+ (this->dataPtr->ellA * this->dataPtr->ellA) *
+ curvature + _pos.Z()) * sinLat);
+ break;
+ }
+
+ // Do nothing
+ case ECEF:
+ break;
+ default:
+ gzerr << "Invalid coordinate type[" << _in << "]\n";
+ return _pos;
+ }
+
+ // Convert ECEF to the requested output coordinate system
+ switch (_out)
+ {
+ case SPHERICAL:
+ {
+ // Convert from ECEF to SPHERICAL
+ double p = sqrt(tmp.X() * tmp.X() + tmp.Y() * tmp.Y());
+ double theta = atan((tmp.Z() * this->dataPtr->ellA) /
+ (p * this->dataPtr->ellB));
+
+ // Calculate latitude and longitude
+ double lat = atan(
+ (tmp.Z() + std::pow(this->dataPtr->ellP, 2) * this->dataPtr->ellB *
+ std::pow(sin(theta), 3)) /
+ (p - std::pow(this->dataPtr->ellE, 2) *
+ this->dataPtr->ellA * std::pow(cos(theta), 3)));
+
+ double lon = atan2(tmp.Y(), tmp.X());
+
+ // Recalculate radius of planet curvature at the current latitude.
+ double nCurvature = 1.0 - std::pow(this->dataPtr->ellE, 2) *
+ std::pow(sin(lat), 2);
+ nCurvature = this->dataPtr->ellA / sqrt(nCurvature);
+
+ tmp.X(lat);
+ tmp.Y(lon);
+
+ // Now calculate Z
+ tmp.Z(p/cos(lat) - nCurvature);
+ break;
+ }
+
+ // Convert from ECEF TO GLOBAL
+ case GLOBAL:
+ tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin);
+ break;
+
+ // Convert from ECEF TO LOCAL
+ case LOCAL:
+ tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin);
+
+ tmp = ignition::math::Vector3d(
+ tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea,
+ tmp.X() * this->dataPtr->sinHea + tmp.Y() * this->dataPtr->cosHea,
+ tmp.Z());
+ break;
+
+ // Return ECEF (do nothing)
+ case ECEF:
+ break;
+
+ default:
+ gzerr << "Unknown coordinate type[" << _out << "]\n";
+ return _pos;
+ }
+
+ return tmp;
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d SphericalCoordinates::VelocityTransform(
+ const ignition::math::Vector3d &_vel,
+ const CoordinateType &_in, const CoordinateType &_out) const
+{
+ // Sanity check -- velocity should not be expressed in spherical coordinates
+ if (_in == SPHERICAL || _out == SPHERICAL)
+ {
+ gzwarn << "Spherical velocities are not supported";
+ return _vel;
+ }
+
+ // Intermediate data type
+ ignition::math::Vector3d tmp = _vel;
+
+ // First, convert to an ECEF vector
+ switch (_in)
+ {
+ // ENU (note no break at end of case)
+ case LOCAL:
+ tmp.X(-_vel.X() * this->dataPtr->cosHea + _vel.Y() *
+ this->dataPtr->sinHea);
+ tmp.Y(-_vel.X() * this->dataPtr->sinHea - _vel.Y() *
+ this->dataPtr->cosHea);
+ // spherical
+ case GLOBAL:
+ tmp = this->dataPtr->rotGlobalToECEF * tmp;
+ break;
+ // Do nothing
+ case ECEF:
+ tmp = _vel;
+ break;
+ default:
+ gzerr << "Unknown coordinate type[" << _in << "]\n";
+ return _vel;
+ }
+
+ // Then, convert to the request coordinate type
+ switch (_out)
+ {
+ // ECEF, do nothing
+ case ECEF:
+ break;
+
+ // Convert from ECEF to global
+ case GLOBAL:
+ tmp = this->dataPtr->rotECEFToGlobal * tmp;
+ break;
+
+ // Convert from ECEF to local
+ case LOCAL:
+ tmp = this->dataPtr->rotECEFToGlobal * tmp;
+ tmp = ignition::math::Vector3d(
+ tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea,
+ tmp.X() * this->dataPtr->sinHea + tmp.Y() * this->dataPtr->cosHea,
+ tmp.Z());
+ break;
+
+ default:
+ gzerr << "Unknown coordinate type[" << _out << "]\n";
+ return _vel;
+ }
+
+ return tmp;
+}
diff --git a/gazebo/common/SphericalCoordinates.hh b/gazebo/common/SphericalCoordinates.hh
index 24ea101..2488cf2 100644
--- a/gazebo/common/SphericalCoordinates.hh
+++ b/gazebo/common/SphericalCoordinates.hh
@@ -25,6 +25,7 @@
#include "gazebo/common/CommonTypes.hh"
#include "gazebo/math/Angle.hh"
#include "gazebo/math/Vector3.hh"
+#include "gazebo/math/Matrix3.hh"
#include "gazebo/util/system.hh"
namespace gazebo
@@ -49,6 +50,23 @@ namespace gazebo
EARTH_WGS84 = 1
};
+ /// \enum CoordinateType
+ /// \brief Unique identifiers for coordinate types.
+ public: enum CoordinateType
+ {
+ /// \brief Latitude, Longitude and Altitude by SurfaceType
+ SPHERICAL = 1,
+
+ /// \brief Earth centered, earth fixed Cartesian
+ ECEF = 2,
+
+ /// \brief Local tangent plane (East, North, Up)
+ GLOBAL = 3,
+
+ /// \brief Heading-adjusted tangent plane (X, Y, Z)
+ LOCAL = 4
+ };
+
/// \brief Constructor.
public: SphericalCoordinates();
@@ -152,7 +170,6 @@ namespace gazebo
const ignition::math::Angle &_latB,
const ignition::math::Angle &_lonB);
-
/// \brief Get SurfaceType currently in use.
/// \return Current SurfaceType value.
public: SurfaceType GetSurfaceType() const;
@@ -236,7 +253,41 @@ namespace gazebo
/// \param[in] _angle Heading offset for gazebo frame.
public: void SetHeadingOffset(const ignition::math::Angle &_angle);
- /// internal
+ /// \brief Convert a geodetic position vector to Cartesian coordinates.
+ /// \param[in] _xyz Geodetic position in the planetary frame of reference
+ /// \return Cartesian vector in Gazebo's world frame
+ public: ignition::math::Vector3d LocalFromSpherical(
+ const ignition::math::Vector3d &_xyz) const;
+
+ /// \brief Convert a Cartesian vector with components East,
+ /// North, Up to a local Gazebo cartesian frame vector XYZ.
+ /// \param[in] Vector with components (x,y,z): (East, North, Up).
+ /// \return Cartesian vector in Gazebo's world frame.
+ public: ignition::math::Vector3d LocalFromGlobal(
+ const ignition::math::Vector3d &_xyz) const;
+
+ /// \brief Update coordinate transformation matrix with reference location
+ public: void UpdateTransformationMatrix();
+
+ /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame
+ /// \param[in] _pos Position vector in frame defined by parameter _in
+ /// \param[in] _in CoordinateType for input
+ /// \param[in] _out CoordinateType for output
+ /// \return Transformed coordinate using cached orgin
+ public: ignition::math::Vector3d
+ PositionTransform(const ignition::math::Vector3d &_pos,
+ const CoordinateType &_in, const CoordinateType &_out) const;
+
+ /// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame
+ /// \param[in] _pos Velocity vector in frame defined by parameter _in
+ /// \param[in] _in CoordinateType for input
+ /// \param[in] _out CoordinateType for output
+ /// \return Transformed velocity vector
+ public: ignition::math::Vector3d VelocityTransform(
+ const ignition::math::Vector3d &_vel,
+ const CoordinateType &_in, const CoordinateType &_out) const;
+
+ /// \internal
/// \brief Pointer to the private data
private: SphericalCoordinatesPrivate *dataPtr;
};
diff --git a/gazebo/common/SphericalCoordinatesPrivate.hh b/gazebo/common/SphericalCoordinatesPrivate.hh
index 994425e..fc4acaf 100644
--- a/gazebo/common/SphericalCoordinatesPrivate.hh
+++ b/gazebo/common/SphericalCoordinatesPrivate.hh
@@ -19,9 +19,10 @@
#define _GAZEBO_SPHERICALCOORDINATES_PRIVATE_HH_
#include <ignition/math/Angle.hh>
+#include <ignition/math/Matrix3.hh>
+#include <ignition/math/Vector3.hh>
#include "gazebo/common/SphericalCoordinates.hh"
-#include "gazebo/math/Angle.hh"
#include "gazebo/util/system.hh"
namespace gazebo
@@ -53,6 +54,36 @@ namespace gazebo
/// \brief Heading offset, expressed as angle from East to
/// gazebo x-axis, or equivalently from North to gazebo y-axis.
public: ignition::math::Angle headingOffset;
+
+ /// \brief Semi-major axis ellipse parameter
+ public: double ellA;
+
+ /// \brief Semi-minor axis ellipse parameter
+ public: double ellB;
+
+ /// \brief Flattening ellipse parameter
+ public: double ellF;
+
+ /// \brief First eccentricity ellipse parameter
+ public: double ellE;
+
+ /// \brief Second eccentricity ellipse parameter
+ public: double ellP;
+
+ /// \brief Rotation matrix that moves ECEF to GLOBAL
+ public: ignition::math::Matrix3d rotECEFToGlobal;
+
+ /// \brief Rotation matrix that moves GLOBAL to ECEF
+ public: ignition::math::Matrix3d rotGlobalToECEF;
+
+ /// \brief Cache the ECEF position of the the origin
+ public: ignition::math::Vector3d origin;
+
+ /// \brief Cache cosine head transform
+ public: double cosHea;
+
+ /// \brief Cache sine head transform
+ public: double sinHea;
};
/// \}
}
diff --git a/gazebo/common/SphericalCoordinates_TEST.cc b/gazebo/common/SphericalCoordinates_TEST.cc
index aa5ed38..ae55ae2 100644
--- a/gazebo/common/SphericalCoordinates_TEST.cc
+++ b/gazebo/common/SphericalCoordinates_TEST.cc
@@ -170,15 +170,80 @@ TEST_F(SphericalCoordinatesTest, CoordinateTransforms)
// elevation
EXPECT_NEAR(sph.Z(), elev, 1e-6);
- // 200 km offset in x (pi/2 heading offset means North)
+ // 200 km offset in x (pi/2 heading offset means North). We use
+ // SphericalFromLocal, which means that xyz is a linear movement on
+ // a plane (not along the curvature of Earth). This will result in
+ // a large height offset.
xyz.Set(2e5, 0, 0);
sph = sc.SphericalFromLocal(xyz);
// increase in latitude about 1.8 degrees
EXPECT_NEAR(sph.X(), lat.Degree() + 1.8, 0.008);
// no change in longitude
- EXPECT_NEAR(sph.Y(), lon.Degree(), 1e-6);
- // no change in elevation
- EXPECT_NEAR(sph.Z(), elev, 1e-6);
+ EXPECT_NEAR(sph.Z(), 3507.024791, 1e-6);
+
+ ignition::math::Vector3d xyz2 = sc.LocalFromSpherical(sph);
+ EXPECT_EQ(xyz, xyz2);
+ }
+
+ // Check position projection
+ {
+ // WGS84 coordinate obtained from online mapping software
+ // > gdaltransform -s_srs WGS84 -t_srs EPSG:4978
+ // > latitude longitude altitude
+ // > X Y Z
+ ignition::math::Vector3d tmp;
+ ignition::math::Vector3d osrf_s(37.3877349, -122.0651166, 32.0);
+ ignition::math::Vector3d osrf_e(
+ -2693701.91434394, -4299942.14687992, 3851691.0393571);
+ ignition::math::Vector3d goog_s(37.4216719, -122.0821853, 30.0);
+ ignition::math::Vector3d goog_e(
+ -2693766.71906146, -4297199.59926038, 3854681.81878812);
+
+ // Local tangent plane coordinates (ENU = GLOBAL) coordinates of
+ // Google when OSRF is taken as the origin:
+ // > proj +ellps=WGS84 +proj=tmerc
+ // +lat_0=37.3877349 +lon_0=-122.0651166 +k=1 +x_0=0 +y_0=0
+ // > -122.0821853 37.4216719 (LON,LAT)
+ // > -1510.88 3766.64 (EAST,NORTH)
+ ignition::math::Vector3d vec(-1510.88, 3766.64, -3.29);
+
+ // Convert degrees to radians
+ osrf_s.X() *= 0.0174532925;
+ osrf_s.Y() *= 0.0174532925;
+
+ // Set the ORIGIN to be the Open Source Robotics Foundation
+ common::SphericalCoordinates sc2(st, ignition::math::Angle(osrf_s.X()),
+ ignition::math::Angle(osrf_s.Y()), osrf_s.Z(), 0.0);
+
+ // Check that SPHERICAL -> ECEF works
+ tmp = sc2.PositionTransform(osrf_s,
+ common::SphericalCoordinates::SPHERICAL,
+ common::SphericalCoordinates::ECEF);
+
+ EXPECT_NEAR(tmp.X(), osrf_e.X(), 8e-2);
+ EXPECT_NEAR(tmp.Y(), osrf_e.Y(), 8e-2);
+ EXPECT_NEAR(tmp.Z(), osrf_e.Z(), 1e-2);
+
+ // Check that ECEF -> SPHERICAL works
+ tmp = sc2.PositionTransform(tmp,
+ common::SphericalCoordinates::ECEF,
+ common::SphericalCoordinates::SPHERICAL);
+
+ EXPECT_NEAR(tmp.X(), osrf_s.X(), 1e-2);
+ EXPECT_NEAR(tmp.Y(), osrf_s.Y(), 1e-2);
+ EXPECT_NEAR(tmp.Z(), osrf_s.Z(), 1e-2);
+
+ // Check that SPHERICAL -> LOCAL works
+ tmp = sc2.LocalFromSpherical(goog_s);
+ EXPECT_NEAR(tmp.X(), vec.X(), 8e-2);
+ EXPECT_NEAR(tmp.Y(), vec.Y(), 8e-2);
+ EXPECT_NEAR(tmp.Z(), vec.Z(), 1e-2);
+
+ // Check that SPHERICAL -> LOCAL -> SPHERICAL works
+ tmp = sc2.SphericalFromLocal(tmp);
+ EXPECT_NEAR(tmp.X(), goog_s.X(), 8e-2);
+ EXPECT_NEAR(tmp.Y(), goog_s.Y(), 8e-2);
+ EXPECT_NEAR(tmp.Z(), goog_s.Z(), 1e-2);
}
}
}
diff --git a/gazebo/gazebo.cc b/gazebo/gazebo.cc
index f93ee6e..746b5d8 100644
--- a/gazebo/gazebo.cc
+++ b/gazebo/gazebo.cc
@@ -22,6 +22,7 @@
#endif
#include <vector>
+#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>
#include <sdf/sdf.hh>
diff --git a/gazebo/gui/ApplyWrenchDialog.cc b/gazebo/gui/ApplyWrenchDialog.cc
index 0d1186e..ba74afb 100644
--- a/gazebo/gui/ApplyWrenchDialog.cc
+++ b/gazebo/gui/ApplyWrenchDialog.cc
@@ -20,6 +20,7 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
diff --git a/gazebo/gui/EntityMaker.hh b/gazebo/gui/EntityMaker.hh
index 680bf9e..3c80f0a 100644
--- a/gazebo/gui/EntityMaker.hh
+++ b/gazebo/gui/EntityMaker.hh
@@ -17,6 +17,7 @@
#ifndef _ENTITYMAKER_HH_
#define _ENTITYMAKER_HH_
+#include <boost/function.hpp>
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/math/Vector3.hh"
diff --git a/gazebo/gui/GLWidget.cc b/gazebo/gui/GLWidget.cc
index edf1a1c..34586d4 100644
--- a/gazebo/gui/GLWidget.cc
+++ b/gazebo/gui/GLWidget.cc
@@ -20,6 +20,8 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
#include <math.h>
#include "gazebo/common/Assert.hh"
diff --git a/gazebo/gui/InsertModelWidget.cc b/gazebo/gui/InsertModelWidget.cc
index 41cf8e1..1bd1f23 100644
--- a/gazebo/gui/InsertModelWidget.cc
+++ b/gazebo/gui/InsertModelWidget.cc
@@ -23,6 +23,7 @@
#include <fstream>
+#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <sdf/sdf.hh>
diff --git a/gazebo/gui/MainWindow.cc b/gazebo/gui/MainWindow.cc
index b506e5c..bf131cc 100644
--- a/gazebo/gui/MainWindow.cc
+++ b/gazebo/gui/MainWindow.cc
@@ -21,6 +21,8 @@
#endif
#include <sdf/sdf.hh>
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
#include <boost/scoped_ptr.hpp>
#include "gazebo/gazebo_config.h"
diff --git a/gazebo/gui/ModelBuilderWidget.cc b/gazebo/gui/ModelBuilderWidget.cc
index e3d44b2..b504eee 100644
--- a/gazebo/gui/ModelBuilderWidget.cc
+++ b/gazebo/gui/ModelBuilderWidget.cc
@@ -21,6 +21,7 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
#include "gazebo/rendering/RenderingIface.hh"
#include "gazebo/rendering/UserCamera.hh"
diff --git a/gazebo/gui/ModelListWidget.cc b/gazebo/gui/ModelListWidget.cc
index 1c29622..dbcdb2f 100644
--- a/gazebo/gui/ModelListWidget.cc
+++ b/gazebo/gui/ModelListWidget.cc
@@ -24,6 +24,8 @@
#include <google/protobuf/descriptor.h>
#include <google/protobuf/message.h>
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/thread/recursive_mutex.hpp>
diff --git a/gazebo/gui/ModelRightMenu.cc b/gazebo/gui/ModelRightMenu.cc
index 4956524..801e6b8 100644
--- a/gazebo/gui/ModelRightMenu.cc
+++ b/gazebo/gui/ModelRightMenu.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/transport/transport.hh"
#include "gazebo/rendering/UserCamera.hh"
#include "gazebo/rendering/Scene.hh"
diff --git a/gazebo/gui/ModelSnap.cc b/gazebo/gui/ModelSnap.cc
index 0ffe156..7641164 100644
--- a/gazebo/gui/ModelSnap.cc
+++ b/gazebo/gui/ModelSnap.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/transport/transport.hh"
#include "gazebo/rendering/RenderTypes.hh"
diff --git a/gazebo/gui/OculusWindow.cc b/gazebo/gui/OculusWindow.cc
index 26b7270..ae723f4 100644
--- a/gazebo/gui/OculusWindow.cc
+++ b/gazebo/gui/OculusWindow.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/gui/OculusWindow.hh"
#include "gazebo/rendering/OculusCamera.hh"
diff --git a/gazebo/gui/QTestFixture.cc b/gazebo/gui/QTestFixture.cc
index ea7353c..a57ec37 100644
--- a/gazebo/gui/QTestFixture.cc
+++ b/gazebo/gui/QTestFixture.cc
@@ -20,6 +20,7 @@
# include <mach/mach.h>
#endif // __MACH__
+#include <boost/bind.hpp>
#include <unistd.h>
#include "gazebo/common/Console.hh"
diff --git a/gazebo/gui/RenderWidget.cc b/gazebo/gui/RenderWidget.cc
index baacb75..e7fc40c 100644
--- a/gazebo/gui/RenderWidget.cc
+++ b/gazebo/gui/RenderWidget.cc
@@ -20,6 +20,8 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
#include <iomanip>
#include "gazebo/rendering/UserCamera.hh"
diff --git a/gazebo/gui/SpaceNav.cc b/gazebo/gui/SpaceNav.cc
index 380430f..2675700 100644
--- a/gazebo/gui/SpaceNav.cc
+++ b/gazebo/gui/SpaceNav.cc
@@ -21,6 +21,7 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
#include <gazebo/gazebo_config.h>
#ifdef HAVE_SPNAV
#include <spnav.h>
diff --git a/gazebo/gui/TimePanel.cc b/gazebo/gui/TimePanel.cc
index aecd279..9b66257 100644
--- a/gazebo/gui/TimePanel.cc
+++ b/gazebo/gui/TimePanel.cc
@@ -21,6 +21,7 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
#include <sstream>
#include "gazebo/transport/Node.hh"
diff --git a/gazebo/gui/TimeWidget_TEST.cc b/gazebo/gui/TimeWidget_TEST.cc
index 5ff4b9f..c190ab9 100644
--- a/gazebo/gui/TimeWidget_TEST.cc
+++ b/gazebo/gui/TimeWidget_TEST.cc
@@ -19,10 +19,161 @@
#include "gazebo/gui/MainWindow.hh"
#include "gazebo/gui/RenderWidget.hh"
#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/Actions.hh"
#include "gazebo/rendering/UserCamera.hh"
#include "gazebo/gui/TimeWidget_TEST.hh"
/////////////////////////////////////////////////
+void TimeWidget_TEST::Reset()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("empty.world", false, false, false);
+
+ // Create the main window.
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Wait a little bit so that time increases.
+ for (unsigned int i = 0; i < 1000; ++i)
+ {
+ gazebo::common::Time::MSleep(1);
+ QCoreApplication::processEvents();
+ }
+
+ // Get the time panel
+ gazebo::gui::TimePanel *timePanel = mainWindow->GetRenderWidget()->
+ GetTimePanel();
+ QVERIFY(timePanel != NULL);
+
+ // Get the time widget
+ gazebo::gui::TimeWidget *timeWidget =
+ timePanel->findChild<gazebo::gui::TimeWidget *>("timeWidget");
+ QVERIFY(timeWidget != NULL);
+ timeWidget->setVisible(true);
+
+ // Get the sim time line edit
+ QLineEdit *simTimeEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetSimTime");
+ QVERIFY(simTimeEdit != NULL);
+
+ // Get the real time line edit
+ QLineEdit *realTimeEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetRealTime");
+ QVERIFY(realTimeEdit != NULL);
+
+ QLineEdit *iterationsEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetIterations");
+ QVERIFY(iterationsEdit != NULL);
+
+ std::string txt;
+ double value;
+
+ // Make sure real time is greater than zero
+ txt = realTimeEdit->text().toStdString();
+ value = boost::lexical_cast<double>(txt.substr(txt.find(".")));
+ QVERIFY(value > 0.0);
+
+ // Make sure sim time is greater than zero
+ txt = simTimeEdit->text().toStdString();
+ value = boost::lexical_cast<double>(txt.substr(txt.find(".")));
+ QVERIFY(value > 0.0);
+
+ // Make sure iteration is greater than zero
+ txt = iterationsEdit->text().toStdString();
+ int intValue = boost::lexical_cast<int>(txt);
+ QVERIFY(intValue > 0);
+
+ // verify reset action is not null
+ QVERIFY(gazebo::gui::g_resetWorldAct != NULL);
+ QVERIFY(gazebo::gui::g_resetWorldAct->isEnabled());
+ QVERIFY(!mainWindow->IsPaused());
+
+ // pause the world before resetting
+ mainWindow->Pause();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ QVERIFY(mainWindow->IsPaused());
+
+ // trigger reset world
+ gazebo::gui::g_resetWorldAct->trigger();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Make sure real time is zero
+ txt = realTimeEdit->text().toStdString();
+ value = boost::lexical_cast<double>(txt.substr(txt.find(".")));
+ QVERIFY(ignition::math::equal(value, 0.0));
+
+ // Make sure sim time is zero
+ txt = simTimeEdit->text().toStdString();
+ value = boost::lexical_cast<double>(txt.substr(txt.find(".")));
+ QVERIFY(ignition::math::equal(value, 0.0));
+
+ // Make sure iteration is zero
+ txt = iterationsEdit->text().toStdString();
+ intValue = boost::lexical_cast<int>(txt);
+ QCOMPARE(intValue, 0);
+
+ // play the simulation and again and verify time increases
+ mainWindow->Play();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ QVERIFY(!mainWindow->IsPaused());
+
+ // Wait a little bit so that time increases.
+ for (unsigned int i = 0; i < 1000; ++i)
+ {
+ gazebo::common::Time::MSleep(1);
+ QCoreApplication::processEvents();
+ }
+
+ // Make sure real time is greater than zero
+ txt = realTimeEdit->text().toStdString();
+ value = boost::lexical_cast<double>(txt.substr(txt.find(".")));
+ QVERIFY(value > 0.0);
+
+ // Make sure sim time is greater than zero
+ txt = simTimeEdit->text().toStdString();
+ value = boost::lexical_cast<double>(txt.substr(txt.find(".")));
+ QVERIFY(value > 0.0);
+
+ // Make sure iteration is greater than zero
+ txt = iterationsEdit->text().toStdString();
+ intValue = boost::lexical_cast<int>(txt);
+ QVERIFY(intValue > 0);
+
+ mainWindow->close();
+ delete mainWindow;
+}
+
+
+/////////////////////////////////////////////////
void TimeWidget_TEST::ValidTimes()
{
this->resMaxPercentChange = 5.0;
diff --git a/gazebo/gui/TimeWidget_TEST.hh b/gazebo/gui/TimeWidget_TEST.hh
index 5daf76b..4877499 100644
--- a/gazebo/gui/TimeWidget_TEST.hh
+++ b/gazebo/gui/TimeWidget_TEST.hh
@@ -25,6 +25,10 @@ class TimeWidget_TEST : public QTestFixture
{
Q_OBJECT
+ /// \brief Test that the TimeWidget gets correct time from the server
+ /// on reset.
+ private slots: void Reset();
+
/// \brief Test that the TimeWidget gets valid times from the server.
private slots: void ValidTimes();
diff --git a/gazebo/gui/ToolsWidget.cc b/gazebo/gui/ToolsWidget.cc
index 672b222..24c833e 100644
--- a/gazebo/gui/ToolsWidget.cc
+++ b/gazebo/gui/ToolsWidget.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/common/Events.hh"
#include "gazebo/gui/JointControlWidget.hh"
#include "gazebo/gui/ToolsWidget.hh"
diff --git a/gazebo/gui/building/BuildingEditor.cc b/gazebo/gui/building/BuildingEditor.cc
index 98f77b7..04f9546 100644
--- a/gazebo/gui/building/BuildingEditor.cc
+++ b/gazebo/gui/building/BuildingEditor.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/gui/qt.h"
#include "gazebo/gui/Actions.hh"
diff --git a/gazebo/gui/building/BuildingEditorPalette.cc b/gazebo/gui/building/BuildingEditorPalette.cc
index 438dd59..b37b70d 100644
--- a/gazebo/gui/building/BuildingEditorPalette.cc
+++ b/gazebo/gui/building/BuildingEditorPalette.cc
@@ -17,6 +17,7 @@
* thenounproject.com
* Stairs designed by Brian Oppenlander from the thenounproject.com
*/
+#include <boost/bind.hpp>
#include "gazebo/gui/building/BuildingEditorPalettePrivate.hh"
#include "gazebo/gui/building/BuildingEditorPalette.hh"
diff --git a/gazebo/gui/building/BuildingMaker.cc b/gazebo/gui/building/BuildingMaker.cc
index 0da2f30..e60d3cb 100644
--- a/gazebo/gui/building/BuildingMaker.cc
+++ b/gazebo/gui/building/BuildingMaker.cc
@@ -23,6 +23,8 @@
#include <sstream>
#include <set>
+#include <boost/bind.hpp>
+#include <boost/filesystem.hpp>
#include "gazebo/common/Exception.hh"
diff --git a/gazebo/gui/building/BuildingModelManip.cc b/gazebo/gui/building/BuildingModelManip.cc
index 0e7e6e5..4fb454a 100644
--- a/gazebo/gui/building/BuildingModelManip.cc
+++ b/gazebo/gui/building/BuildingModelManip.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/rendering/Visual.hh"
#include "gazebo/common/Exception.hh"
diff --git a/gazebo/gui/building/EditorView.cc b/gazebo/gui/building/EditorView.cc
index 3f05c1b..5cf4183 100644
--- a/gazebo/gui/building/EditorView.cc
+++ b/gazebo/gui/building/EditorView.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/math/Angle.hh"
#include "gazebo/gui/building/ImportImageDialog.hh"
diff --git a/gazebo/gui/building/LevelWidget.cc b/gazebo/gui/building/LevelWidget.cc
index 1d8a32e..52811c1 100644
--- a/gazebo/gui/building/LevelWidget.cc
+++ b/gazebo/gui/building/LevelWidget.cc
@@ -15,6 +15,7 @@
*
*/
+#include <boost/bind.hpp>
#include <sstream>
#include "gazebo/gui/building/LevelWidget.hh"
#include "gazebo/gui/building/BuildingEditorEvents.hh"
diff --git a/gazebo/gui/building/ScaleWidget.cc b/gazebo/gui/building/ScaleWidget.cc
index 7668dc8..c91f035 100644
--- a/gazebo/gui/building/ScaleWidget.cc
+++ b/gazebo/gui/building/ScaleWidget.cc
@@ -15,6 +15,7 @@
*
*/
+#include <boost/bind.hpp>
#include <sstream>
#include "gazebo/gui/building/BuildingEditorEvents.hh"
#include "gazebo/gui/building/ScaleWidget.hh"
diff --git a/gazebo/gui/model/JointMaker.cc b/gazebo/gui/model/JointMaker.cc
index 1f27103..7f5f304 100644
--- a/gazebo/gui/model/JointMaker.cc
+++ b/gazebo/gui/model/JointMaker.cc
@@ -15,6 +15,7 @@
*
*/
+#include <boost/bind.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <string>
#include <vector>
diff --git a/gazebo/gui/model/ModelCreator.cc b/gazebo/gui/model/ModelCreator.cc
index 7e075dd..f8ec740 100644
--- a/gazebo/gui/model/ModelCreator.cc
+++ b/gazebo/gui/model/ModelCreator.cc
@@ -21,6 +21,7 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/filesystem.hpp>
#include <sstream>
diff --git a/gazebo/gui/model/ModelEditor.cc b/gazebo/gui/model/ModelEditor.cc
index 2eae41a..1df44f4 100644
--- a/gazebo/gui/model/ModelEditor.cc
+++ b/gazebo/gui/model/ModelEditor.cc
@@ -15,6 +15,7 @@
*
*/
+#include <boost/bind.hpp>
#include <string>
#include "gazebo/gazebo_config.h"
diff --git a/gazebo/gui/model/ModelEditorPalette.cc b/gazebo/gui/model/ModelEditorPalette.cc
index 0bb9dc1..f882fed 100644
--- a/gazebo/gui/model/ModelEditorPalette.cc
+++ b/gazebo/gui/model/ModelEditorPalette.cc
@@ -15,6 +15,7 @@
*
*/
+#include <boost/bind.hpp>
#include <string>
#include "gazebo/rendering/DynamicLines.hh"
diff --git a/gazebo/gui/terrain/TerrainEditorPalette.cc b/gazebo/gui/terrain/TerrainEditorPalette.cc
index 679c032..1b8ec8d 100644
--- a/gazebo/gui/terrain/TerrainEditorPalette.cc
+++ b/gazebo/gui/terrain/TerrainEditorPalette.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/rendering/Heightmap.hh"
#include "gazebo/rendering/Scene.hh"
diff --git a/gazebo/gui/viewers/CMakeLists.txt b/gazebo/gui/viewers/CMakeLists.txt
index 1cfa093..66c7fc4 100644
--- a/gazebo/gui/viewers/CMakeLists.txt
+++ b/gazebo/gui/viewers/CMakeLists.txt
@@ -27,6 +27,7 @@ set (qt_headers
set (qt_tests
ImagesView_TEST.cc
+ LaserView_TEST.cc
)
# Generate executables for each of the QT unit tests
diff --git a/gazebo/gui/viewers/LaserView.cc b/gazebo/gui/viewers/LaserView.cc
index 8d95523..334938c 100644
--- a/gazebo/gui/viewers/LaserView.cc
+++ b/gazebo/gui/viewers/LaserView.cc
@@ -238,8 +238,15 @@ void LaserView::LaserItem::paint(QPainter *_painter,
boost::mutex::scoped_lock lock(this->mutex);
QColor orange(245, 129, 19, 255);
+ QColor noHitDarkGrey(200, 200, 200, 255);
+ QColor noHitLightGrey(200, 200, 200, 50);
QColor darkGrey(100, 100, 100, 255);
QColor lightGrey(100, 100, 100, 50);
+ _painter->setPen(QPen(noHitDarkGrey));
+ _painter->setBrush(noHitLightGrey);
+
+ // Draw the filled polygon that fill the gaps for the laser rays
+ _painter->drawPolygon(&this->noHitPoints[0], this->noHitPoints.size());
_painter->setPen(QPen(darkGrey));
_painter->setBrush(lightGrey);
@@ -266,7 +273,9 @@ void LaserView::LaserItem::paint(QPainter *_painter,
{
double x1, y1;
- double rangeScaled = this->ranges[index] * this->scale;
+ double r = this->ranges[index];
+ double hitRange = std::isinf(r) ? 0 : r;
+ double rangeScaled = hitRange * this->scale;
double rangeMaxScaled = this->rangeMax * this->scale;
this->indexAngle = this->angleMin + index * this->angleStep;
@@ -417,6 +426,7 @@ void LaserView::LaserItem::Clear()
boost::mutex::scoped_lock lock(this->mutex);
this->ranges.clear();
this->points.clear();
+ this->noHitPoints.clear();
}
/////////////////////////////////////////////////
@@ -453,6 +463,7 @@ void LaserView::LaserItem::Update(double _angleMin, double _angleMax,
// A min range > 0 means we have to draw an inner circle, so we twice as
// many points
this->points.resize(this->ranges.size() * 2);
+ this->noHitPoints.resize(this->ranges.size() * 2);
}
else if (math::equal(this->rangeMin, 0.0) &&
this->ranges.size() + 1 != this->points.size())
@@ -460,6 +471,7 @@ void LaserView::LaserItem::Update(double _angleMin, double _angleMax,
// A min range == 0 mean we just need a closing point at the (0, 0)
// location
this->points.resize(this->ranges.size() + 1);
+ this->noHitPoints.resize(this->ranges.size() + 1);
}
double angle = this->angleMin;
@@ -467,9 +479,16 @@ void LaserView::LaserItem::Update(double _angleMin, double _angleMax,
// Add a point for each laser reading
for (unsigned int i = 0; i < this->ranges.size(); ++i)
{
- QPointF pt(this->ranges[i] * this->scale * cos(angle),
- -this->ranges[i] * this->scale * sin(angle));
+ double r = this->ranges[i];
+ double hitRange = std::isinf(r) ? 0 : r;
+ QPointF pt(hitRange * this->scale * cos(angle),
+ -hitRange * this->scale * sin(angle));
this->points[i] = pt;
+ double noHitRange = std::isinf(r) ? this->rangeMax : hitRange;
+ QPointF noHitPt(noHitRange * this->scale * cos(angle),
+ -noHitRange * this->scale * sin(angle));
+ this->noHitPoints[i] = noHitPt;
+
angle += this->angleStep;
}
@@ -482,6 +501,7 @@ void LaserView::LaserItem::Update(double _angleMin, double _angleMax,
QPointF pt(this->rangeMin * this->scale * cos(angle),
-this->rangeMin * this->scale * sin(angle));
this->points[i + this->ranges.size()] = pt;
+ this->noHitPoints[i + this->ranges.size()] = pt;
angle -= this->angleStep;
}
}
@@ -489,6 +509,7 @@ void LaserView::LaserItem::Update(double _angleMin, double _angleMax,
{
// Connect the last point to the (0,0)
this->points[this->ranges.size()] = QPointF(0, 0);
+ this->noHitPoints[this->ranges.size()] = QPointF(0, 0);
}
// Tell QT we have changed.
diff --git a/gazebo/gui/viewers/LaserView.hh b/gazebo/gui/viewers/LaserView.hh
index d33df7b..9ec7c37 100644
--- a/gazebo/gui/viewers/LaserView.hh
+++ b/gazebo/gui/viewers/LaserView.hh
@@ -145,6 +145,9 @@ namespace gazebo
/// \brief The vector of points to draw.
private: std::vector<QPointF> points;
+ /// \brief The vector of points to draw for rays with no hit.
+ private: std::vector<QPointF> noHitPoints;
+
/// \brief The vector of range values, as returned by the
/// laser sensor.
private: std::vector<double> ranges;
diff --git a/gazebo/gui/viewers/LaserView_TEST.cc b/gazebo/gui/viewers/LaserView_TEST.cc
new file mode 100644
index 0000000..59f13fd
--- /dev/null
+++ b/gazebo/gui/viewers/LaserView_TEST.cc
@@ -0,0 +1,107 @@
+/*
+ * 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 "gazebo/math/Rand.hh"
+#include "gazebo/gui/viewers/LaserView_TEST.hh"
+
+/////////////////////////////////////////////////
+void LaserView_TEST::Construction()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/ray_test.world");
+
+ // Create a new laser view widget
+ gazebo::gui::LaserView *view = new gazebo::gui::LaserView(NULL);
+
+ QVERIFY(view != NULL);
+
+ view->show();
+
+ // Get the frame that holds the images
+ QFrame *frame = view->findChild<QFrame*>("blackBorderFrame");
+
+ QVERIFY(frame != NULL);
+
+ view->SetTopic("~/hokuyo/link/laser/scan");
+
+ // Spin the Qt update loop for a while to process events.
+ for (int j = 0; j < 50; ++j)
+ {
+ gazebo::common::Time::MSleep(gazebo::math::Rand::GetIntUniform(10, 50));
+ QCoreApplication::processEvents();
+ }
+
+ view->hide();
+ delete view;
+}
+
+/////////////////////////////////////////////////
+void LaserView_TEST::Buttons()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/ray_test.world");
+
+ // Create a new laser view widget
+ gazebo::gui::LaserView *view = new gazebo::gui::LaserView(NULL);
+
+ QVERIFY(view != NULL);
+
+ view->show();
+
+ view->SetTopic("~/hokuyo/link/laser/scan");
+
+ // Spin the Qt update loop for a while to process events.
+ for (int j = 0; j < 50; ++j)
+ {
+ gazebo::common::Time::MSleep(10);
+ QCoreApplication::processEvents();
+ }
+
+ // Get Fit in View button and click it
+ QPushButton *pushButton = view->findChild<QPushButton *>();
+ QVERIFY(pushButton != NULL);
+ pushButton->click();
+
+ // Spin the Qt update loop for a while to process events.
+ for (int j = 0; j < 50; ++j)
+ {
+ gazebo::common::Time::MSleep(10);
+ QCoreApplication::processEvents();
+ }
+
+ // Get Degrees radio button and toggle it
+ QRadioButton *radioButton = view->findChild<QRadioButton *>();
+ QVERIFY(radioButton != NULL);
+ radioButton->toggle();
+
+ // Spin the Qt update loop for a while to process events.
+ for (int j = 0; j < 50; ++j)
+ {
+ gazebo::common::Time::MSleep(10);
+ QCoreApplication::processEvents();
+ }
+
+ view->hide();
+ delete view;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(LaserView_TEST)
diff --git a/gazebo/gui/TimeWidget_TEST.hh b/gazebo/gui/viewers/LaserView_TEST.hh
similarity index 58%
copy from gazebo/gui/TimeWidget_TEST.hh
copy to gazebo/gui/viewers/LaserView_TEST.hh
index 5daf76b..3c79632 100644
--- a/gazebo/gui/TimeWidget_TEST.hh
+++ b/gazebo/gui/viewers/LaserView_TEST.hh
@@ -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,21 +15,21 @@
*
*/
-#ifndef _TIMEPANEL_TEST_HH_
-#define _TIMEPANEL_TEST_HH_
+#ifndef _GAZEBO_LASERVIEW_TEST_HH_
+#define _GAZEBO_LASERVIEW_TEST_HH_
#include "gazebo/gui/QTestFixture.hh"
+#include "gazebo/gui/viewers/LaserView.hh"
-/// \brief A test class for the TimeWidget widget.
-class TimeWidget_TEST : public QTestFixture
+/// \brief A test class for the LaserView widget.
+class LaserView_TEST : public QTestFixture
{
Q_OBJECT
- /// \brief Test that the TimeWidget gets valid times from the server.
- private slots: void ValidTimes();
+ /// \brief Test construction and usage in a world with a laser.
+ private slots: void Construction();
- /// \brief Test setting the visibility of TimeWidget child widgets.
- private slots: void Visibility();
+ /// \brief Test invoking buttons in the laser view widget.
+ private slots: void Buttons();
};
-
#endif
diff --git a/gazebo/physics/ContactManager.cc b/gazebo/physics/ContactManager.cc
index 4861f98..72344ce 100644
--- a/gazebo/physics/ContactManager.cc
+++ b/gazebo/physics/ContactManager.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
#include "gazebo/transport/TransportIface.hh"
diff --git a/gazebo/physics/Entity.cc b/gazebo/physics/Entity.cc
index d6eedbc..f0c717d 100644
--- a/gazebo/physics/Entity.cc
+++ b/gazebo/physics/Entity.cc
@@ -25,6 +25,8 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include "gazebo/msgs/msgs.hh"
diff --git a/gazebo/physics/Entity.hh b/gazebo/physics/Entity.hh
index e7f06ef..1035c76 100644
--- a/gazebo/physics/Entity.hh
+++ b/gazebo/physics/Entity.hh
@@ -25,6 +25,7 @@
#include <string>
#include <vector>
+#include <boost/function.hpp>
#include "gazebo/msgs/msgs.hh"
#include "gazebo/transport/TransportTypes.hh"
diff --git a/gazebo/physics/Gripper.cc b/gazebo/physics/Gripper.cc
index f95525a..060f5a7 100644
--- a/gazebo/physics/Gripper.cc
+++ b/gazebo/physics/Gripper.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/common/Events.hh"
#include "gazebo/transport/Node.hh"
diff --git a/gazebo/physics/Inertial.cc b/gazebo/physics/Inertial.cc
index 015f1f1..4ce1501 100644
--- a/gazebo/physics/Inertial.cc
+++ b/gazebo/physics/Inertial.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/physics/Inertial.hh"
using namespace gazebo;
diff --git a/gazebo/physics/Link.cc b/gazebo/physics/Link.cc
index 0fa01f0..3b4464c 100644
--- a/gazebo/physics/Link.cc
+++ b/gazebo/physics/Link.cc
@@ -21,6 +21,8 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
#include <sstream>
#include "gazebo/msgs/msgs.hh"
diff --git a/gazebo/physics/Model.cc b/gazebo/physics/Model.cc
index b18068b..bcfd317 100644
--- a/gazebo/physics/Model.cc
+++ b/gazebo/physics/Model.cc
@@ -25,6 +25,8 @@
#include <tbb/blocked_range.h>
#include <float.h>
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <sstream>
diff --git a/gazebo/physics/Model.hh b/gazebo/physics/Model.hh
index 98e327c..7b75269 100644
--- a/gazebo/physics/Model.hh
+++ b/gazebo/physics/Model.hh
@@ -25,6 +25,7 @@
#include <string>
#include <map>
#include <vector>
+#include <boost/function.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include "gazebo/common/CommonTypes.hh"
diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc
index aa489b3..56d4ae6 100644
--- a/gazebo/physics/World.cc
+++ b/gazebo/physics/World.cc
@@ -26,6 +26,7 @@
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
+#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/recursive_mutex.hpp>
@@ -690,7 +691,8 @@ void World::Step()
this->dataPtr->prevStepWallTime = common::Time::GetWallTime();
double stepTime = this->dataPtr->physicsEngine->GetMaxStepSize();
- if (!this->IsPaused() || this->dataPtr->stepInc > 0)
+ if (!this->IsPaused() || this->dataPtr->stepInc > 0
+ || this->dataPtr->needsReset)
{
// query timestep to allow dynamic time step size updates
this->dataPtr->simTime += stepTime;
@@ -1083,6 +1085,10 @@ void World::ResetTime()
this->dataPtr->startTime = common::Time::GetWallTime();
this->dataPtr->realTimeOffset = common::Time(0);
this->dataPtr->iterations = 0;
+
+ if (this->IsPaused())
+ this->dataPtr->pauseStartTime = this->dataPtr->startTime;
+
sensors::SensorManager::Instance()->ResetLastUpdateTimes();
}
diff --git a/gazebo/physics/bullet/BulletJoint.cc b/gazebo/physics/bullet/BulletJoint.cc
index 178afd2..57652c3 100644
--- a/gazebo/physics/bullet/BulletJoint.cc
+++ b/gazebo/physics/bullet/BulletJoint.cc
@@ -19,6 +19,7 @@
* Date: 15 May 2009
*/
+#include <boost/bind.hpp>
#include <string>
#include "gazebo/common/Assert.hh"
diff --git a/gazebo/physics/bullet/BulletLink.cc b/gazebo/physics/bullet/BulletLink.cc
index 6cbfdb1..8ac3fea 100644
--- a/gazebo/physics/bullet/BulletLink.cc
+++ b/gazebo/physics/bullet/BulletLink.cc
@@ -215,6 +215,16 @@ void BulletLink::Fini()
bulletWorld->removeRigidBody(this->rigidLink);
}
+/////////////////////////////////////////////////////////////////////
+void BulletLink::UpdateMass()
+{
+ if (this->rigidLink && this->inertial)
+ {
+ this->rigidLink->setMassProps(this->inertial->GetMass(),
+ BulletTypes::ConvertVector3(this->inertial->GetPrincipalMoments()));
+ }
+}
+
//////////////////////////////////////////////////
void BulletLink::SetGravityMode(bool _mode)
{
diff --git a/gazebo/physics/bullet/BulletLink.hh b/gazebo/physics/bullet/BulletLink.hh
index d9722bc..cef6db4 100644
--- a/gazebo/physics/bullet/BulletLink.hh
+++ b/gazebo/physics/bullet/BulletLink.hh
@@ -159,6 +159,9 @@ namespace gazebo
// Documentation inherited
public: virtual void SetLinkStatic(bool _static);
+ // Documentation inherited.
+ public: virtual void UpdateMass();
+
/// \brief Pointer to bullet compound shape, which is a container
/// for other child shapes.
private: btCollisionShape *compoundShape;
diff --git a/gazebo/physics/dart/DARTJoint.cc b/gazebo/physics/dart/DARTJoint.cc
index dc08d50..a3b59e0 100644
--- a/gazebo/physics/dart/DARTJoint.cc
+++ b/gazebo/physics/dart/DARTJoint.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
diff --git a/gazebo/physics/dart/DARTLink.cc b/gazebo/physics/dart/DARTLink.cc
index e3cecfb..3063d3c 100644
--- a/gazebo/physics/dart/DARTLink.cc
+++ b/gazebo/physics/dart/DARTLink.cc
@@ -260,6 +260,22 @@ void DARTLink::Fini()
Link::Fini();
}
+/////////////////////////////////////////////////////////////////////
+void DARTLink::UpdateMass()
+{
+ if (this->dataPtr->dtBodyNode && this->inertial)
+ {
+ this->dataPtr->dtBodyNode->setMass(this->inertial->GetMass());
+ auto Ixxyyzz = this->inertial->GetPrincipalMoments();
+ auto Ixyxzyz = this->inertial->GetProductsofInertia();
+ this->dataPtr->dtBodyNode->setMomentOfInertia(
+ Ixxyyzz[0], Ixxyyzz[1], Ixxyyzz[2],
+ Ixyxzyz[0], Ixyxzyz[1], Ixyxzyz[2]);
+ auto cog = DARTTypes::ConvVec3(this->inertial->GetCoG());
+ this->dataPtr->dtBodyNode->setLocalCOM(cog);
+ }
+}
+
//////////////////////////////////////////////////
void DARTLink::OnPoseChange()
{
diff --git a/gazebo/physics/dart/DARTLink.hh b/gazebo/physics/dart/DARTLink.hh
index c92023c..a0d57f8 100644
--- a/gazebo/physics/dart/DARTLink.hh
+++ b/gazebo/physics/dart/DARTLink.hh
@@ -179,6 +179,9 @@ namespace gazebo
/// \param[in] _dartChildJoint Pointer to the child joint.
public: void AddDARTChildJoint(DARTJointPtr _dartChildJoint);
+ // Documentation inherited.
+ public: virtual void UpdateMass();
+
/// \internal
/// \brief Pointer to private data
private: DARTLinkPrivate *dataPtr;
diff --git a/gazebo/physics/dart/DARTScrewJoint.cc b/gazebo/physics/dart/DARTScrewJoint.cc
index d7f0dc5..f915d49 100644
--- a/gazebo/physics/dart/DARTScrewJoint.cc
+++ b/gazebo/physics/dart/DARTScrewJoint.cc
@@ -147,6 +147,7 @@ void DARTScrewJoint::SetThreadPitch(unsigned int _index, double _threadPitch)
{
if (_index >= this->GetAngleCount())
gzerr << "Invalid index[" << _index << "]\n";
+
this->SetThreadPitch(_threadPitch);
}
@@ -157,7 +158,7 @@ void DARTScrewJoint::SetThreadPitch(double _threadPitch)
reinterpret_cast<dart::dynamics::ScrewJoint *>(this->dataPtr->dtJoint);
this->threadPitch = _threadPitch;
- dtScrewJoint->setPitch(_threadPitch * 2.0 * M_PI);
+ dtScrewJoint->setPitch(DARTTypes::InvertThreadPitch(_threadPitch));
}
//////////////////////////////////////////////////
@@ -165,6 +166,7 @@ double DARTScrewJoint::GetThreadPitch(unsigned int _index)
{
if (_index >= this->GetAngleCount())
gzerr << "Invalid index[" << _index << "]\n";
+
return this->GetThreadPitch();
}
@@ -176,9 +178,10 @@ double DARTScrewJoint::GetThreadPitch()
double result = this->threadPitch;
if (dtScrewJoint)
- result = dtScrewJoint->getPitch() * 0.5 / M_PI;
+ result = DARTTypes::InvertThreadPitch(dtScrewJoint->getPitch());
else
gzwarn << "dartScrewJoint not created yet, returning cached threadPitch.\n";
+
return result;
}
@@ -204,13 +207,12 @@ math::Angle DARTScrewJoint::GetAngleImpl(unsigned int _index) const
}
else if (_index == 1)
{
- dart::dynamics::ScrewJoint *dtScrewJoint =
- reinterpret_cast<dart::dynamics::ScrewJoint *>(
- this->dataPtr->dtJoint);
-
// linear position
const double radianAngle = this->dataPtr->dtJoint->getPosition(0);
- result = dtScrewJoint->getPitch() * radianAngle * 0.5 / M_PI;
+ result.SetFromRadian(-radianAngle /
+ const_cast<DARTScrewJoint*>(this)->GetThreadPitch());
+ // TODO: The ScrewJoint::GetThreadPitch() function is not const. As an
+ // workaround, we use const_cast here until #1686 is resolved.
}
else
{
diff --git a/gazebo/physics/dart/DARTTypes.hh b/gazebo/physics/dart/DARTTypes.hh
index 9e96a6c..067fcfb 100644
--- a/gazebo/physics/dart/DARTTypes.hh
+++ b/gazebo/physics/dart/DARTTypes.hh
@@ -19,6 +19,7 @@
#define _GAZEBO_DARTTYPES_HH_
#include <boost/shared_ptr.hpp>
+#include "gazebo/common/Assert.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/physics/dart/dart_inc.h"
#include "gazebo/util/system.hh"
@@ -102,6 +103,20 @@ namespace gazebo
pose.rot = ConvQuat(Eigen::Quaterniond(_T.linear()));
return pose;
}
+
+ /// \brief Invert thread pitch to match the different definitions of
+ /// thread pitch in Gazebo and DART.
+ ///
+ /// [Definitions of thread pitch]
+ /// Gazebo: NEGATIVE angular motion per linear motion.
+ /// DART : linear motion per single rotation.
+ public: static double InvertThreadPitch(double _pitch)
+ {
+ GZ_ASSERT(std::abs(_pitch) > 0.0,
+ "Zero thread pitch is not allowed.\n");
+
+ return -2.0 * M_PI / _pitch;
+ }
};
}
}
diff --git a/gazebo/physics/ode/ODEJoint.cc b/gazebo/physics/ode/ODEJoint.cc
index c45f05e..53a5a34 100644
--- a/gazebo/physics/ode/ODEJoint.cc
+++ b/gazebo/physics/ode/ODEJoint.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Assert.hh"
@@ -429,10 +430,6 @@ bool ODEJoint::SetParam(const std::string &_key, unsigned int _index,
}
else if (_key == "fmax")
{
- gzwarn << "The '" << _key << "' parameter is deprecated "
- << "to enable Coulomb joint friction with the "
- << "'friction' parameter"
- << std::endl;
this->SetParam(dParamFMax | group, boost::any_cast<double>(_value));
}
else if (_key == "friction")
@@ -445,10 +442,6 @@ bool ODEJoint::SetParam(const std::string &_key, unsigned int _index,
}
else if (_key == "vel")
{
- gzwarn << "The '" << _key << "' parameter is deprecated "
- << "to enable Coulomb joint friction with the "
- << "'friction' parameter"
- << std::endl;
this->SetParam(dParamVel | group, boost::any_cast<double>(_value));
}
else if (_key == "hi_stop")
@@ -559,10 +552,6 @@ double ODEJoint::GetParam(const std::string &_key, unsigned int _index)
}
else if (_key == "fmax")
{
- gzwarn << "The '" << _key << "' parameter is deprecated "
- << "to enable Coulomb joint friction with the "
- << "'friction' parameter"
- << std::endl;
return this->GetParam(dParamFMax | group);
}
else if (_key == "friction")
@@ -571,10 +560,6 @@ double ODEJoint::GetParam(const std::string &_key, unsigned int _index)
}
else if (_key == "vel")
{
- gzwarn << "The '" << _key << "' parameter is deprecated "
- << "to enable Coulomb joint friction with the "
- << "'friction' parameter"
- << std::endl;
return this->GetParam(dParamVel | group);
}
else if (_key == "hi_stop")
diff --git a/gazebo/physics/ode/ODELink.cc b/gazebo/physics/ode/ODELink.cc
index f784ffd..9f88914 100644
--- a/gazebo/physics/ode/ODELink.cc
+++ b/gazebo/physics/ode/ODELink.cc
@@ -95,48 +95,14 @@ void ODELink::Init()
{
GZ_ASSERT(this->inertial != NULL, "Inertial pointer is NULL");
math::Vector3 cogVec = this->inertial->GetCoG();
- Base_V::iterator iter;
- for (iter = this->children.begin(); iter != this->children.end(); ++iter)
+ for (auto const &child : this->children)
{
- if ((*iter)->HasType(Base::COLLISION))
+ if (child->HasType(Base::COLLISION))
{
- ODECollisionPtr g = boost::static_pointer_cast<ODECollision>(*iter);
+ ODECollisionPtr g = boost::static_pointer_cast<ODECollision>(child);
if (g->IsPlaceable() && g->GetCollisionId())
{
dGeomSetBody(g->GetCollisionId(), this->linkId);
-
- // update pose immediately
- math::Pose localPose = g->GetRelativePose();
- localPose.pos -= cogVec;
-
- dQuaternion q;
- q[0] = localPose.rot.w;
- q[1] = localPose.rot.x;
- q[2] = localPose.rot.y;
- q[3] = localPose.rot.z;
-
- // Set the pose of the encapsulated collision; this is always relative
- // to the CoM
- dGeomSetOffsetPosition(g->GetCollisionId(), localPose.pos.x,
- localPose.pos.y, localPose.pos.z);
- dGeomSetOffsetQuaternion(g->GetCollisionId(), q);
-
- // Set max_vel and min_depth
- boost::any value;
- if (g->GetODESurface()->maxVel < 0 && this->GetWorld()->
- GetPhysicsEngine()->GetParam("contact_max_correcting_vel", value))
- {
- try
- {
- g->GetODESurface()->maxVel = boost::any_cast<double>(value);
- }
- catch(boost::bad_any_cast &_e)
- {
- gzerr << "Failed boost::any_cast in ODELink.cc: " << _e.what();
- }
- }
- dBodySetMaxVel(this->linkId, g->GetODESurface()->maxVel);
- dBodySetMinDepth(this->linkId, g->GetODESurface()->minDepth);
}
}
}
@@ -148,7 +114,9 @@ void ODELink::Init()
<< " in ODELink::Init" << std::endl;
}
- // Update the Center of Mass.
+ // Update the Collision Offsets, Surface, and Center of Mass.
+ this->UpdateCollisionOffsets();
+ this->UpdateSurface();
this->UpdateMass();
if (this->linkId)
@@ -337,17 +305,69 @@ bool ODELink::GetEnabled() const
}
/////////////////////////////////////////////////////////////////////
+void ODELink::UpdateCollisionOffsets()
+{
+ if (this->linkId)
+ {
+ GZ_ASSERT(this->inertial != NULL, "Inertial pointer is NULL");
+ math::Vector3 cogVec = this->inertial->GetCoG();
+ for (auto const &child : this->children)
+ {
+ if (child->HasType(Base::COLLISION))
+ {
+ ODECollisionPtr g = boost::static_pointer_cast<ODECollision>(child);
+ if (g->IsPlaceable() && g->GetCollisionId())
+ {
+ // update pose immediately
+ math::Pose localPose = g->GetRelativePose();
+ localPose.pos -= cogVec;
+
+ dQuaternion q;
+ q[0] = localPose.rot.w;
+ q[1] = localPose.rot.x;
+ q[2] = localPose.rot.y;
+ q[3] = localPose.rot.z;
+
+ // Set the pose of the encapsulated collision; this is always relative
+ // to the CoM
+ dGeomSetOffsetPosition(g->GetCollisionId(), localPose.pos.x,
+ localPose.pos.y, localPose.pos.z);
+ dGeomSetOffsetQuaternion(g->GetCollisionId(), q);
+ }
+ }
+ }
+ }
+}
+
+/////////////////////////////////////////////////////////////////////
void ODELink::UpdateSurface()
{
- Base_V::iterator iter;
- Base_V::iterator iter_end = this->children.end();
- for (iter = this->children.begin(); iter != iter_end; ++iter)
+ if (!this->linkId)
+ {
+ return;
+ }
+
+ for (auto const &child : this->children)
{
- if ((*iter)->HasType(Base::COLLISION))
+ if (child->HasType(Base::COLLISION))
{
- ODECollisionPtr g = boost::static_pointer_cast<ODECollision>(*iter);
+ ODECollisionPtr g = boost::static_pointer_cast<ODECollision>(child);
if (g->IsPlaceable() && g->GetCollisionId())
{
+ // Set max_vel and min_depth
+ boost::any value;
+ if (g->GetODESurface()->maxVel < 0 && this->GetWorld()->
+ GetPhysicsEngine()->GetParam("contact_max_correcting_vel", value))
+ {
+ try
+ {
+ g->GetODESurface()->maxVel = boost::any_cast<double>(value);
+ }
+ catch(boost::bad_any_cast &_e)
+ {
+ gzerr << "Failed boost::any_cast in ODELink.cc: " << _e.what();
+ }
+ }
// Set surface properties max_vel and min_depth
dBodySetMaxVel(this->linkId, g->GetODESurface()->maxVel);
dBodySetMinDepth(this->linkId, g->GetODESurface()->minDepth);
@@ -355,6 +375,7 @@ void ODELink::UpdateSurface()
}
}
}
+
/////////////////////////////////////////////////////////////////////
void ODELink::UpdateMass()
{
@@ -388,6 +409,10 @@ void ODELink::UpdateMass()
dBodySetMass(this->linkId, &odeMass);
else
gzthrow("Setting custom link " + this->GetScopedName() + "mass to zero!");
+
+ // In case the center of mass changed:
+ this->UpdateCollisionOffsets();
+ this->OnPoseChange();
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/ode/ODELink.hh b/gazebo/physics/ode/ODELink.hh
index ea0aa46..93d8cde 100644
--- a/gazebo/physics/ode/ODELink.hh
+++ b/gazebo/physics/ode/ODELink.hh
@@ -58,6 +58,12 @@ namespace gazebo
// Documentation inherited
public: virtual bool GetEnabled() const;
+ /// \brief Update location of collisions relative to center of mass.
+ /// This used to be done only in the Init function, but was moved
+ /// to a separate function to handle dynamic updates to the
+ /// center of mass.
+ public: void UpdateCollisionOffsets();
+
// Documentation inherited
public: virtual void UpdateMass();
diff --git a/gazebo/physics/ode/ODEPhysics.cc b/gazebo/physics/ode/ODEPhysics.cc
index de1be62..11943de 100644
--- a/gazebo/physics/ode/ODEPhysics.cc
+++ b/gazebo/physics/ode/ODEPhysics.cc
@@ -626,6 +626,8 @@ ODEPhysics::ConvertWorldStepSolverType(const std::string &_solverType)
result = ODE_DEFAULT;
else if (_solverType.compare("DART_PGS") == 0)
result = DART_PGS;
+ else if (_solverType.compare("BULLET_LEMKE") == 0)
+ result = BULLET_LEMKE;
else if (_solverType.compare("BULLET_PGS") == 0)
result = BULLET_PGS;
else
@@ -655,6 +657,11 @@ ODEPhysics::ConvertWorldStepSolverType(const World_Solver_Type _solverType)
result = "DART_PGS";
break;
}
+ case BULLET_LEMKE:
+ {
+ result = "BULLET_LEMKE";
+ break;
+ }
case BULLET_PGS:
{
result = "BULLET_PGS";
diff --git a/gazebo/physics/ode/ODEPhysics.hh b/gazebo/physics/ode/ODEPhysics.hh
index 79c6a08..83f558a 100644
--- a/gazebo/physics/ode/ODEPhysics.hh
+++ b/gazebo/physics/ode/ODEPhysics.hh
@@ -254,13 +254,13 @@ namespace gazebo
/// \brief Convert a World_Solver_Type enum to a string.
/// \param[in] _solverType World_Solver_Type enum.
- /// \return world solver type string. Returns "unknown" if
+ /// \return World solver type string. Returns "unknown" if
/// _solverType is unrecognized.
public: static std::string
ConvertWorldStepSolverType(const World_Solver_Type _solverType);
/// \brief Convert a string to a World_Solver_Type enum.
- /// \param[in] _solverType world solver type string.
+ /// \param[in] _solverType World solver type string.
/// \return A World_Solver_Type enum. Defaults to ODE_DEFAULT
/// if _solverType is unrecognized.
public: static World_Solver_Type
diff --git a/gazebo/physics/simbody/SimbodyLink.cc b/gazebo/physics/simbody/SimbodyLink.cc
index 7aea53b..0568af4 100644
--- a/gazebo/physics/simbody/SimbodyLink.cc
+++ b/gazebo/physics/simbody/SimbodyLink.cc
@@ -19,6 +19,7 @@
* Date: 13 Feb 2006
*/
+#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include "gazebo/common/Assert.hh"
@@ -115,6 +116,11 @@ void SimbodyLink::Fini()
Link::Fini();
}
+/////////////////////////////////////////////////////////////////////
+void SimbodyLink::UpdateMass()
+{
+}
+
//////////////////////////////////////////////////
void SimbodyLink::SetGravityMode(bool _mode)
{
diff --git a/gazebo/physics/simbody/SimbodyLink.hh b/gazebo/physics/simbody/SimbodyLink.hh
index 9d790c4..1c41852 100644
--- a/gazebo/physics/simbody/SimbodyLink.hh
+++ b/gazebo/physics/simbody/SimbodyLink.hh
@@ -161,6 +161,9 @@ namespace gazebo
public: void SetDirtyPose(const math::Pose &_pose);
+ // Documentation inherited.
+ public: virtual void UpdateMass();
+
/// \brief Internal call to change effect of gravity on Link
/// based on gravityMode if gravityModeDirty is true.
private: void ProcessSetGravityMode();
diff --git a/gazebo/rendering/COMVisual.cc b/gazebo/rendering/COMVisual.cc
index 52654c4..c5d245b 100644
--- a/gazebo/rendering/COMVisual.cc
+++ b/gazebo/rendering/COMVisual.cc
@@ -42,6 +42,26 @@ COMVisual::COMVisual(const std::string &_name, VisualPtr _vis)
/////////////////////////////////////////////////
COMVisual::~COMVisual()
{
+ COMVisualPrivate *dPtr =
+ reinterpret_cast<COMVisualPrivate *>(this->dataPtr);
+ if (dPtr && dPtr->sceneNode)
+ {
+ this->DestroyAllAttachedMovableObjects(dPtr->sceneNode);
+ dPtr->sceneNode->removeAndDestroyAllChildren();
+ }
+}
+
+/////////////////////////////////////////////////
+void COMVisual::Fini()
+{
+ COMVisualPrivate *dPtr =
+ reinterpret_cast<COMVisualPrivate *>(this->dataPtr);
+ if (dPtr && dPtr->sceneNode)
+ {
+ this->DestroyAllAttachedMovableObjects(dPtr->sceneNode);
+ dPtr->sceneNode->removeAndDestroyAllChildren();
+ }
+ Visual::Fini();
}
/////////////////////////////////////////////////
@@ -132,7 +152,7 @@ void COMVisual::Load()
sphereObj->setCastShadows(false);
dPtr->sphereNode =
- dPtr->sceneNode->createChildSceneNode(this->GetName() + "_SPHERE");
+ dPtr->sceneNode->createChildSceneNode(this->GetName() + "_SPHERE_");
dPtr->sphereNode->attachObject(sphereObj);
dPtr->sphereNode->setScale(sphereRadius*2, sphereRadius*2, sphereRadius*2);
@@ -179,3 +199,33 @@ math::Pose COMVisual::GetInertiaPose() const
return dPtr->inertiaPose;
}
+
+/////////////////////////////////////////////////
+void COMVisual::DestroyAllAttachedMovableObjects(Ogre::SceneNode *_sceneNode)
+{
+ if (!_sceneNode)
+ return;
+
+ // Destroy all the attached objects
+ Ogre::SceneNode::ObjectIterator itObject =
+ _sceneNode->getAttachedObjectIterator();
+
+ while (itObject.hasMoreElements())
+ {
+ Ogre::Entity *ent = static_cast<Ogre::Entity*>(itObject.getNext());
+ if (ent->getMovableType() != DynamicLines::GetMovableType())
+ this->dataPtr->scene->GetManager()->destroyEntity(ent);
+ else
+ delete ent;
+ }
+
+ // Recurse to child SceneNodes
+ Ogre::SceneNode::ChildNodeIterator itChild = _sceneNode->getChildIterator();
+
+ while (itChild.hasMoreElements())
+ {
+ Ogre::SceneNode* pChildNode =
+ static_cast<Ogre::SceneNode*>(itChild.getNext());
+ this->DestroyAllAttachedMovableObjects(pChildNode);
+ }
+}
diff --git a/gazebo/rendering/COMVisual.hh b/gazebo/rendering/COMVisual.hh
index 8d96b23..6d06d1a 100644
--- a/gazebo/rendering/COMVisual.hh
+++ b/gazebo/rendering/COMVisual.hh
@@ -41,7 +41,10 @@ namespace gazebo
public: COMVisual(const std::string &_name, VisualPtr _vis);
/// \brief Destructor
- public: virtual ~COMVisual();
+ public: ~COMVisual();
+
+ // Inherited from parent class
+ public: virtual void Fini();
/// \brief Load the Visual from an SDF pointer
/// \param[in] _elem SDF Element pointer
@@ -58,6 +61,11 @@ namespace gazebo
/// \brief Load using previously set member variables.
private: void Load();
+
+ /// \brief Destroy all the movable objects attached to a scene node.
+ /// \param[in] _sceneNode Pointer to the scene node to process.
+ private: void DestroyAllAttachedMovableObjects(
+ Ogre::SceneNode *_sceneNode);
};
/// \}
}
diff --git a/gazebo/rendering/Camera.cc b/gazebo/rendering/Camera.cc
index be9039d..5000e71 100644
--- a/gazebo/rendering/Camera.cc
+++ b/gazebo/rendering/Camera.cc
@@ -15,17 +15,23 @@
*
*/
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+#endif
+
#include <sstream>
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
+#include <boost/function.hpp>
#include <sdf/sdf.hh>
#ifndef _WIN32
#include <dirent.h>
#else
- // Ensure that Winsock2.h is included before Windows.h, which can get
- // pulled in by anybody (e.g., Boost).
- #include <Winsock2.h>
#include "gazebo/common/win_dirent.h"
#endif
diff --git a/gazebo/rendering/Camera.hh b/gazebo/rendering/Camera.hh
index 4edf00f..9258d81 100644
--- a/gazebo/rendering/Camera.hh
+++ b/gazebo/rendering/Camera.hh
@@ -19,6 +19,7 @@
#define _GAZEBO_RENDERING_CAMERA_HH_
#include <boost/enable_shared_from_this.hpp>
+#include <boost/function.hpp>
#include <string>
#include <utility>
#include <list>
diff --git a/gazebo/rendering/CameraVisual.cc b/gazebo/rendering/CameraVisual.cc
index 1878f06..0fa5260 100644
--- a/gazebo/rendering/CameraVisual.cc
+++ b/gazebo/rendering/CameraVisual.cc
@@ -21,6 +21,8 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/DynamicLines.hh"
#include "gazebo/rendering/Scene.hh"
diff --git a/gazebo/rendering/ContactVisual.cc b/gazebo/rendering/ContactVisual.cc
index 4ab4b10..e1d4305 100644
--- a/gazebo/rendering/ContactVisual.cc
+++ b/gazebo/rendering/ContactVisual.cc
@@ -17,13 +17,14 @@
/* Desc: Contact Visualization Class
* Author: Nate Koenig
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/common/MeshManager.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Subscriber.hh"
diff --git a/gazebo/rendering/InertiaVisual.cc b/gazebo/rendering/InertiaVisual.cc
index 411d3b4..16bfa11 100644
--- a/gazebo/rendering/InertiaVisual.cc
+++ b/gazebo/rendering/InertiaVisual.cc
@@ -39,6 +39,26 @@ InertiaVisual::InertiaVisual(const std::string &_name, VisualPtr _vis)
/////////////////////////////////////////////////
InertiaVisual::~InertiaVisual()
{
+ InertiaVisualPrivate *dPtr =
+ reinterpret_cast<InertiaVisualPrivate *>(this->dataPtr);
+ if (dPtr && dPtr->sceneNode)
+ {
+ this->DestroyAllAttachedMovableObjects(dPtr->sceneNode);
+ dPtr->sceneNode->removeAndDestroyAllChildren();
+ }
+}
+
+/////////////////////////////////////////////////
+void InertiaVisual::Fini()
+{
+ InertiaVisualPrivate *dPtr =
+ reinterpret_cast<InertiaVisualPrivate *>(this->dataPtr);
+ if (dPtr && dPtr->sceneNode)
+ {
+ this->DestroyAllAttachedMovableObjects(dPtr->sceneNode);
+ dPtr->sceneNode->removeAndDestroyAllChildren();
+ }
+ Visual::Fini();
}
/////////////////////////////////////////////////
@@ -137,7 +157,7 @@ void InertiaVisual::Load(const math::Pose &_pose,
((Ogre::Entity*)boxObj)->setMaterialName("__GAZEBO_TRANS_PURPLE_MATERIAL__");
dPtr->boxNode =
- dPtr->sceneNode->createChildSceneNode(this->GetName() + "_BOX");
+ dPtr->sceneNode->createChildSceneNode(this->GetName() + "_BOX_");
dPtr->boxNode->attachObject(boxObj);
dPtr->boxNode->setScale(_scale.x, _scale.y, _scale.z);
@@ -147,3 +167,34 @@ void InertiaVisual::Load(const math::Pose &_pose,
this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
}
+
+/////////////////////////////////////////////////
+void InertiaVisual::DestroyAllAttachedMovableObjects(
+ Ogre::SceneNode *_sceneNode)
+{
+ if (!_sceneNode)
+ return;
+
+ // Destroy all the attached objects
+ Ogre::SceneNode::ObjectIterator itObject =
+ _sceneNode->getAttachedObjectIterator();
+
+ while (itObject.hasMoreElements())
+ {
+ Ogre::Entity *ent = static_cast<Ogre::Entity*>(itObject.getNext());
+ if (ent->getMovableType() != DynamicLines::GetMovableType())
+ this->dataPtr->scene->GetManager()->destroyEntity(ent);
+ else
+ delete ent;
+ }
+
+ // Recurse to child SceneNodes
+ Ogre::SceneNode::ChildNodeIterator itChild = _sceneNode->getChildIterator();
+
+ while (itChild.hasMoreElements())
+ {
+ Ogre::SceneNode* pChildNode =
+ static_cast<Ogre::SceneNode*>(itChild.getNext());
+ this->DestroyAllAttachedMovableObjects(pChildNode);
+ }
+}
diff --git a/gazebo/rendering/InertiaVisual.hh b/gazebo/rendering/InertiaVisual.hh
index 2b1f2f1..dcc6ef1 100644
--- a/gazebo/rendering/InertiaVisual.hh
+++ b/gazebo/rendering/InertiaVisual.hh
@@ -40,7 +40,10 @@ namespace gazebo
public: InertiaVisual(const std::string &_name, VisualPtr _vis);
/// \brief Destructor
- public: virtual ~InertiaVisual();
+ public: ~InertiaVisual();
+
+ // Inherited from parent class
+ public: virtual void Fini();
/// \brief Load the Visual from an SDF pointer
/// \param[in] _elem SDF Element pointer
@@ -56,6 +59,11 @@ namespace gazebo
/// \param[in] _scale Scale factor for the box visual.
private: void Load(const math::Pose &_pose,
const math::Vector3 &_scale = math::Vector3(0.02, 0.02, 0.02));
+
+ /// \brief Destroy all the movable objects attached to a scene node.
+ /// \param[in] _sceneNode Pointer to the scene node to process.
+ private: void DestroyAllAttachedMovableObjects(
+ Ogre::SceneNode *_sceneNode);
};
/// \}
}
diff --git a/gazebo/rendering/LaserVisual.cc b/gazebo/rendering/LaserVisual.cc
index 5d5cb77..f49a210 100644
--- a/gazebo/rendering/LaserVisual.cc
+++ b/gazebo/rendering/LaserVisual.cc
@@ -21,6 +21,8 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/common/MeshManager.hh"
#include "gazebo/transport/transport.hh"
diff --git a/gazebo/rendering/Projector.cc b/gazebo/rendering/Projector.cc
index 080022d..d3264fd 100644
--- a/gazebo/rendering/Projector.cc
+++ b/gazebo/rendering/Projector.cc
@@ -18,6 +18,8 @@
* Desc: Projector
* Author: Jared Duke, John Hsu, Nate Koenig
*/
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
#include "gazebo/rendering/RTShaderSystem.hh"
diff --git a/gazebo/rendering/RenderEngine.cc b/gazebo/rendering/RenderEngine.cc
index 064f90c..f5aa59d 100644
--- a/gazebo/rendering/RenderEngine.cc
+++ b/gazebo/rendering/RenderEngine.cc
@@ -14,9 +14,15 @@
* limitations under the License.
*
*/
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+#endif
#include <string>
#include <iostream>
+#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
#include <sys/types.h>
@@ -34,9 +40,6 @@
#ifndef _WIN32
#include <dirent.h>
#else
- // Ensure that Winsock2.h is included before Windows.h, which can get
- // pulled in by anybody (e.g., Boost).
- #include <Winsock2.h>
#include "gazebo/common/win_dirent.h"
#endif
diff --git a/gazebo/rendering/Road2d.cc b/gazebo/rendering/Road2d.cc
index e093cfe..0d3d74a 100644
--- a/gazebo/rendering/Road2d.cc
+++ b/gazebo/rendering/Road2d.cc
@@ -21,6 +21,7 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
#include <algorithm>
#include "gazebo/transport/transport.hh"
diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc
index 09d8b95..7f18a41 100644
--- a/gazebo/rendering/Scene.cc
+++ b/gazebo/rendering/Scene.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include "gazebo/rendering/skyx/include/SkyX.h"
@@ -340,8 +341,7 @@ void Scene::Init()
// Create Fog
if (this->dataPtr->sdf->HasElement("fog"))
{
- boost::shared_ptr<sdf::Element> fogElem =
- this->dataPtr->sdf->GetElement("fog");
+ sdf::ElementPtr fogElem = this->dataPtr->sdf->GetElement("fog");
this->SetFog(fogElem->Get<std::string>("type"),
fogElem->Get<common::Color>("color"),
fogElem->Get<double>("density"),
@@ -2104,6 +2104,10 @@ bool Scene::ProcessSensorMsg(ConstSensorPtr &_msg)
cameraVis->Load(_msg->logical_camera());
this->dataPtr->visuals[cameraVis->GetId()] = cameraVis;
}
+ else if (_msg->has_pose())
+ {
+ iter->second->SetPose(msgs::ConvertIgn(_msg->pose()));
+ }
}
else if (_msg->type() == "contact" && _msg->visualize() &&
!_msg->topic().empty())
diff --git a/gazebo/rendering/SonarVisual.cc b/gazebo/rendering/SonarVisual.cc
index cca7caa..60a1035 100644
--- a/gazebo/rendering/SonarVisual.cc
+++ b/gazebo/rendering/SonarVisual.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/common/MeshManager.hh"
#include "gazebo/transport/transport.hh"
diff --git a/gazebo/rendering/TransmitterVisual.cc b/gazebo/rendering/TransmitterVisual.cc
index e86481e..805887d 100644
--- a/gazebo/rendering/TransmitterVisual.cc
+++ b/gazebo/rendering/TransmitterVisual.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/transport/transport.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/DynamicLines.hh"
diff --git a/gazebo/rendering/UserCamera.cc b/gazebo/rendering/UserCamera.cc
index b3c9c85..966ae21 100644
--- a/gazebo/rendering/UserCamera.cc
+++ b/gazebo/rendering/UserCamera.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/common/Assert.hh"
diff --git a/gazebo/rendering/VideoVisual.cc b/gazebo/rendering/VideoVisual.cc
index 162641e..9e1047f 100644
--- a/gazebo/rendering/VideoVisual.cc
+++ b/gazebo/rendering/VideoVisual.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include "gazebo/common/Video.hh"
#include "gazebo/common/Events.hh"
diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc
index 840cdd4..a109528 100644
--- a/gazebo/rendering/Visual.cc
+++ b/gazebo/rendering/Visual.cc
@@ -14,6 +14,8 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/msgs/msgs.hh"
@@ -184,6 +186,7 @@ Visual::~Visual()
if (this->dataPtr->sceneNode != NULL)
{
+ // seems we never get into this block because Fini() runs first
this->DestroyAllAttachedMovableObjects(this->dataPtr->sceneNode);
this->dataPtr->sceneNode->removeAndDestroyAllChildren();
this->dataPtr->scene->GetManager()->destroySceneNode(
@@ -1342,8 +1345,7 @@ void Visual::SetWireframe(bool _show)
//////////////////////////////////////////////////
void Visual::SetTransparencyInnerLoop(Ogre::SceneNode *_sceneNode)
{
- for (unsigned int i = 0; i < _sceneNode->numAttachedObjects();
- i++)
+ for (unsigned int i = 0; i < _sceneNode->numAttachedObjects(); ++i)
{
Ogre::Entity *entity = NULL;
Ogre::MovableObject *obj = _sceneNode->getAttachedObject(i);
@@ -1357,7 +1359,7 @@ void Visual::SetTransparencyInnerLoop(Ogre::SceneNode *_sceneNode)
continue;
// For each ogre::entity
- for (unsigned int j = 0; j < entity->getNumSubEntities(); j++)
+ for (unsigned int j = 0; j < entity->getNumSubEntities(); ++j)
{
Ogre::SubEntity *subEntity = entity->getSubEntity(j);
Ogre::MaterialPtr material = subEntity->getMaterial();
@@ -1399,15 +1401,18 @@ void Visual::SetTransparencyInnerLoop(Ogre::SceneNode *_sceneNode)
pass->setDiffuse(dc);
this->dataPtr->diffuse = Conversions::Convert(dc);
-
for (unitStateCount = 0; unitStateCount <
pass->getNumTextureUnitStates(); ++unitStateCount)
{
auto textureUnitState = pass->getTextureUnitState(unitStateCount);
- textureUnitState->setAlphaOperation(
- Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT,
- 1.0 - this->dataPtr->transparency);
+ if (textureUnitState->getColourBlendMode().operation ==
+ Ogre::LBX_SOURCE1)
+ {
+ textureUnitState->setAlphaOperation(
+ Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT,
+ 1.0 - this->dataPtr->transparency);
+ }
}
}
}
diff --git a/gazebo/rendering/Visual.hh b/gazebo/rendering/Visual.hh
index 1107034..f117a40 100644
--- a/gazebo/rendering/Visual.hh
+++ b/gazebo/rendering/Visual.hh
@@ -19,6 +19,7 @@
#define _GAZEBO_VISUAL_HH_
#include <boost/enable_shared_from_this.hpp>
+#include <boost/function.hpp>
#include <string>
#include <utility>
#include <vector>
diff --git a/gazebo/rendering/VisualPrivate.hh b/gazebo/rendering/VisualPrivate.hh
index 242d0ea..3627f36 100644
--- a/gazebo/rendering/VisualPrivate.hh
+++ b/gazebo/rendering/VisualPrivate.hh
@@ -23,6 +23,7 @@
#include <list>
#include <vector>
+#include <boost/function.hpp>
#include <sdf/sdf.hh>
#include "gazebo/msgs/msgs.hh"
diff --git a/gazebo/rendering/WrenchVisual.cc b/gazebo/rendering/WrenchVisual.cc
index 3c0f254..97e7aba 100644
--- a/gazebo/rendering/WrenchVisual.cc
+++ b/gazebo/rendering/WrenchVisual.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/common/MeshManager.hh"
#include "gazebo/transport/transport.hh"
diff --git a/gazebo/sensors/AltimeterSensor.cc b/gazebo/sensors/AltimeterSensor.cc
index 7dda153..bbbaa8d 100644
--- a/gazebo/sensors/AltimeterSensor.cc
+++ b/gazebo/sensors/AltimeterSensor.cc
@@ -20,6 +20,7 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
#include "gazebo/common/common.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc
index 8c76bbb..99f9805 100644
--- a/gazebo/sensors/CameraSensor.cc
+++ b/gazebo/sensors/CameraSensor.cc
@@ -20,6 +20,9 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
+
#include "gazebo/common/Events.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Image.hh"
diff --git a/gazebo/sensors/ContactSensor.cc b/gazebo/sensors/ContactSensor.cc
index 35eee05..1cf065d 100644
--- a/gazebo/sensors/ContactSensor.cc
+++ b/gazebo/sensors/ContactSensor.cc
@@ -25,6 +25,7 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
#include <sstream>
#include "gazebo/common/Exception.hh"
diff --git a/gazebo/sensors/DepthCameraSensor.cc b/gazebo/sensors/DepthCameraSensor.cc
index 996d0e7..69b265a 100644
--- a/gazebo/sensors/DepthCameraSensor.cc
+++ b/gazebo/sensors/DepthCameraSensor.cc
@@ -20,6 +20,7 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
#include <sstream>
#include "gazebo/physics/World.hh"
diff --git a/gazebo/sensors/ForceTorqueSensor.cc b/gazebo/sensors/ForceTorqueSensor.cc
index 065a404..a4cc9ec 100644
--- a/gazebo/sensors/ForceTorqueSensor.cc
+++ b/gazebo/sensors/ForceTorqueSensor.cc
@@ -20,6 +20,8 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+
#include "gazebo/physics/World.hh"
#include "gazebo/physics/PhysicsEngine.hh"
#include "gazebo/physics/Joint.hh"
diff --git a/gazebo/sensors/GpsSensor.cc b/gazebo/sensors/GpsSensor.cc
index d9b3336..5c73568 100644
--- a/gazebo/sensors/GpsSensor.cc
+++ b/gazebo/sensors/GpsSensor.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+
#include "gazebo/sensors/SensorFactory.hh"
#include "gazebo/common/common.hh"
diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc
index c67ba99..24b2005 100644
--- a/gazebo/sensors/GpuRaySensor.cc
+++ b/gazebo/sensors/GpuRaySensor.cc
@@ -20,6 +20,9 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
#include "gazebo/physics/World.hh"
#include "gazebo/physics/Entity.hh"
#include "gazebo/physics/Model.hh"
diff --git a/gazebo/sensors/GpuRaySensor.hh b/gazebo/sensors/GpuRaySensor.hh
index ab8c3ad..e6f1c9e 100644
--- a/gazebo/sensors/GpuRaySensor.hh
+++ b/gazebo/sensors/GpuRaySensor.hh
@@ -24,6 +24,7 @@
#include <vector>
#include <string>
+#include <boost/function.hpp>
#include <boost/thread/mutex.hpp>
#include <ignition/math/Angle.hh>
diff --git a/gazebo/sensors/GpuRaySensor_TEST.cc b/gazebo/sensors/GpuRaySensor_TEST.cc
index 41052c2..1f8e4b7 100644
--- a/gazebo/sensors/GpuRaySensor_TEST.cc
+++ b/gazebo/sensors/GpuRaySensor_TEST.cc
@@ -15,6 +15,7 @@
*
*/
+#include <boost/bind.hpp>
#include <gtest/gtest.h>
#include <ignition/math/Angle.hh>
#include "gazebo/test/ServerFixture.hh"
diff --git a/gazebo/sensors/ImuSensor.cc b/gazebo/sensors/ImuSensor.cc
index 45f86e9..4ce03b8 100644
--- a/gazebo/sensors/ImuSensor.cc
+++ b/gazebo/sensors/ImuSensor.cc
@@ -20,6 +20,7 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
#include <ignition/math/Rand.hh>
#include "gazebo/transport/Node.hh"
diff --git a/gazebo/sensors/LogicalCameraSensor.cc b/gazebo/sensors/LogicalCameraSensor.cc
index d1e5f3b..7a1e748 100644
--- a/gazebo/sensors/LogicalCameraSensor.cc
+++ b/gazebo/sensors/LogicalCameraSensor.cc
@@ -20,6 +20,7 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
#include "gazebo/transport/transport.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/World.hh"
diff --git a/gazebo/sensors/MagnetometerSensor.cc b/gazebo/sensors/MagnetometerSensor.cc
index c1d310d..7428356 100644
--- a/gazebo/sensors/MagnetometerSensor.cc
+++ b/gazebo/sensors/MagnetometerSensor.cc
@@ -21,6 +21,7 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
#include <ignition/math/Pose3.hh>
#include "gazebo/transport/Node.hh"
diff --git a/gazebo/sensors/MultiCameraSensor.cc b/gazebo/sensors/MultiCameraSensor.cc
index be17bea..fbb24fe 100644
--- a/gazebo/sensors/MultiCameraSensor.cc
+++ b/gazebo/sensors/MultiCameraSensor.cc
@@ -20,6 +20,8 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
#include <ignition/math/Pose3.hh>
#include "gazebo/common/Exception.hh"
diff --git a/gazebo/sensors/Noise.cc b/gazebo/sensors/Noise.cc
index db0b767..cb74e2b 100644
--- a/gazebo/sensors/Noise.cc
+++ b/gazebo/sensors/Noise.cc
@@ -21,6 +21,7 @@
#include <Winsock2.h>
#endif
+#include <boost/function.hpp>
#include "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
diff --git a/gazebo/sensors/Noise.hh b/gazebo/sensors/Noise.hh
index 793f423..334960f 100644
--- a/gazebo/sensors/Noise.hh
+++ b/gazebo/sensors/Noise.hh
@@ -21,6 +21,7 @@
#include <vector>
#include <string>
+#include <boost/function.hpp>
#include <sdf/sdf.hh>
#include "gazebo/rendering/RenderTypes.hh"
diff --git a/gazebo/sensors/Noise_TEST.cc b/gazebo/sensors/Noise_TEST.cc
index 11de1bc..0f4553f 100644
--- a/gazebo/sensors/Noise_TEST.cc
+++ b/gazebo/sensors/Noise_TEST.cc
@@ -21,6 +21,7 @@
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
+#include <boost/bind.hpp>
#include <ignition/math/Rand.hh>
diff --git a/gazebo/sensors/RaySensor.cc b/gazebo/sensors/RaySensor.cc
index fef761e..a1ac366 100644
--- a/gazebo/sensors/RaySensor.cc
+++ b/gazebo/sensors/RaySensor.cc
@@ -20,6 +20,8 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+
#include "gazebo/physics/World.hh"
#include "gazebo/physics/MultiRayShape.hh"
#include "gazebo/physics/PhysicsEngine.hh"
diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc
index 49fc8fc..9290eab 100644
--- a/gazebo/sensors/Sensor.cc
+++ b/gazebo/sensors/Sensor.cc
@@ -299,6 +299,26 @@ ignition::math::Pose3d Sensor::Pose() const
}
//////////////////////////////////////////////////
+void Sensor::SetPose(const ignition::math::Pose3d &_pose)
+{
+ this->pose = _pose;
+
+ // Update the visualization with the pose information.
+ if (this->sensorPub && this->GetVisualize())
+ {
+ msgs::Sensor msg;
+ msg.set_name(this->GetName());
+ msg.set_id(this->GetId());
+ msg.set_parent(this->GetParentName());
+ msg.set_parent_id(this->GetParentId());
+ msg.set_type(this->GetType());
+ msg.set_visualize(true);
+ msgs::Set(msg.mutable_pose(), this->pose);
+ this->sensorPub->Publish(msg);
+ }
+}
+
+//////////////////////////////////////////////////
double Sensor::GetUpdateRate()
{
if (this->updatePeriod.Double() > 0.0)
@@ -360,8 +380,10 @@ void Sensor::FillMsg(msgs::Sensor &_msg)
_msg.set_parent_id(this->GetParentId());
msgs::Set(_msg.mutable_pose(), this->Pose());
- _msg.set_visualize(this->GetVisualize());
+ _msg.set_always_on(this->IsActive());
_msg.set_topic(this->GetTopic());
+ _msg.set_update_rate(this->GetUpdateRate());
+ _msg.set_visualize(this->GetVisualize());
if (this->GetType() == "logical_camera")
{
@@ -376,10 +398,14 @@ void Sensor::FillMsg(msgs::Sensor &_msg)
{
CameraSensor *camSensor = static_cast<CameraSensor*>(this);
msgs::CameraSensor *camMsg = _msg.mutable_camera();
+ auto cam = camSensor->GetCamera();
+ camMsg->set_horizontal_fov(cam->GetHFOV().Radian());
camMsg->mutable_image_size()->set_x(camSensor->GetImageWidth());
camMsg->mutable_image_size()->set_y(camSensor->GetImageHeight());
- rendering::DistortionPtr distortion =
- camSensor->GetCamera()->GetDistortion();
+ camMsg->set_image_format(cam->GetImageFormat());
+ camMsg->set_near_clip(cam->GetNearClip());
+ camMsg->set_far_clip(cam->GetFarClip());
+ auto distortion = cam->GetDistortion();
if (distortion)
{
msgs::Distortion *distortionMsg = camMsg->mutable_distortion();
diff --git a/gazebo/sensors/Sensor.hh b/gazebo/sensors/Sensor.hh
index 40d4588..b2b9571 100644
--- a/gazebo/sensors/Sensor.hh
+++ b/gazebo/sensors/Sensor.hh
@@ -137,8 +137,17 @@ namespace gazebo
/// \brief Get the current pose.
/// \return Current pose of the sensor.
+ /// \sa SetPose()
public: virtual ignition::math::Pose3d Pose() const;
+ /// \brief Set the current pose.
+ /// TODO: this method is declared non virtual to fix an ABI
+ /// problem detected in gazebo6 series, it will be virtual
+ /// from gazebo7 on.
+ /// \param[in] _pose New pose of the sensor.
+ /// \sa Pose()
+ public: void SetPose(const ignition::math::Pose3d &_pose);
+
/// \brief Set whether the sensor is active or not.
/// \param[in] _value True if active, false if not.
public: virtual void SetActive(bool _value);
diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc
index 6f64beb..5e375b6 100644
--- a/gazebo/sensors/SensorManager.cc
+++ b/gazebo/sensors/SensorManager.cc
@@ -20,6 +20,7 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
#include "gazebo/common/Assert.hh"
#include "gazebo/common/Time.hh"
diff --git a/gazebo/sensors/Sensor_TEST.cc b/gazebo/sensors/Sensor_TEST.cc
index d8488ec..2644915 100644
--- a/gazebo/sensors/Sensor_TEST.cc
+++ b/gazebo/sensors/Sensor_TEST.cc
@@ -198,6 +198,15 @@ TEST_F(Sensor_TEST, UpdateAfterReset)
}
/////////////////////////////////////////////////
+/// \brief Set pose
+TEST_F(Sensor_TEST, SetPose)
+{
+ sensors::Sensor sensor(gazebo::sensors::OTHER);
+ sensor.SetPose(ignition::math::Pose3d(0, 1, 2, 3, 4, 5));
+ EXPECT_EQ(sensor.Pose(), ignition::math::Pose3d(0, 1, 2, 3, 4, 5));
+}
+
+/////////////////////////////////////////////////
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
diff --git a/gazebo/sensors/SonarSensor.cc b/gazebo/sensors/SonarSensor.cc
index 55bc911..0f92df8 100644
--- a/gazebo/sensors/SonarSensor.cc
+++ b/gazebo/sensors/SonarSensor.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
+
#include <ignition/math/Vector3.hh>
#include "gazebo/physics/World.hh"
diff --git a/gazebo/sensors/WirelessReceiver_TEST.cc b/gazebo/sensors/WirelessReceiver_TEST.cc
index cdfd76f..084854d 100644
--- a/gazebo/sensors/WirelessReceiver_TEST.cc
+++ b/gazebo/sensors/WirelessReceiver_TEST.cc
@@ -16,7 +16,7 @@
*/
#include <gtest/gtest.h>
-#include <boost/algorithm/string/find.hpp>
+#include <boost/algorithm/string.hpp>
#include <boost/regex.hpp>
#include "gazebo/test/ServerFixture.hh"
diff --git a/gazebo/sensors/WirelessTransceiver.cc b/gazebo/sensors/WirelessTransceiver.cc
index f929ae5..d6f440b 100644
--- a/gazebo/sensors/WirelessTransceiver.cc
+++ b/gazebo/sensors/WirelessTransceiver.cc
@@ -20,6 +20,7 @@
#include <Winsock2.h>
#endif
+#include <boost/algorithm/string.hpp>
#include <sstream>
#include "gazebo/msgs/msgs.hh"
#include "gazebo/sensors/SensorFactory.hh"
diff --git a/gazebo/test/helper_physics_generator.hh b/gazebo/test/helper_physics_generator.hh
index 68984f1..cf55d41 100644
--- a/gazebo/test/helper_physics_generator.hh
+++ b/gazebo/test/helper_physics_generator.hh
@@ -21,10 +21,18 @@
#include "gazebo/gazebo_config.h"
#define BULLET_SUPPORT
+#define WORLD_STEP_BULLET_LEMKE
+#define WORLD_STEP_BULLET_PGS
#ifdef HAVE_BULLET
# undef BULLET_SUPPORT
# define BULLET_SUPPORT , "bullet"
+# undef WORLD_STEP_BULLET_PGS
+# define WORLD_STEP_BULLET_PGS , "BULLET_PGS"
+# ifdef LIBBULLET_VERSION_GT_282
+# undef WORLD_STEP_BULLET_LEMKE
+# define WORLD_STEP_BULLET_LEMKE , "BULLET_LEMKE"
+# endif
#endif
#define SIMBODY_SUPPORT
@@ -52,6 +60,8 @@
/// \brief Helper macro to instantiate gtest for different solvers
#define WORLD_STEP_SOLVERS ::testing::Values("ODE_DANTZIG" \
WORLD_STEP_DART_PGS \
+ WORLD_STEP_BULLET_PGS \
+ WORLD_STEP_BULLET_LEMKE \
)
#endif
diff --git a/gazebo/transport/Connection.cc b/gazebo/transport/Connection.cc
index 257ab7e..c0166de 100644
--- a/gazebo/transport/Connection.cc
+++ b/gazebo/transport/Connection.cc
@@ -46,6 +46,8 @@
#include <stdio.h>
#include <stdlib.h>
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
#include <boost/lexical_cast.hpp>
#include "gazebo/common/Console.hh"
diff --git a/gazebo/transport/ConnectionManager.cc b/gazebo/transport/ConnectionManager.cc
index 95bb8f7..fba7405 100644
--- a/gazebo/transport/ConnectionManager.cc
+++ b/gazebo/transport/ConnectionManager.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/msgs/msgs.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Events.hh"
diff --git a/gazebo/transport/IOManager.cc b/gazebo/transport/IOManager.cc
index 1234f77..66a35d0 100644
--- a/gazebo/transport/IOManager.cc
+++ b/gazebo/transport/IOManager.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <boost/bind.hpp>
#include <iostream>
#include "gazebo/transport/IOManager.hh"
diff --git a/gazebo/transport/Node.cc b/gazebo/transport/Node.cc
index 419d5d1..87a6f3b 100644
--- a/gazebo/transport/Node.cc
+++ b/gazebo/transport/Node.cc
@@ -22,6 +22,7 @@
#endif
#include <boost/algorithm/string.hpp>
+#include <boost/bind.hpp>
#include "gazebo/transport/TransportIface.hh"
#include "gazebo/transport/Node.hh"
diff --git a/gazebo/transport/Node.hh b/gazebo/transport/Node.hh
index ffd9a1e..37e3de4 100644
--- a/gazebo/transport/Node.hh
+++ b/gazebo/transport/Node.hh
@@ -19,6 +19,7 @@
#define _NODE_HH_
#include <tbb/task.h>
+#include <boost/bind.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <map>
#include <list>
diff --git a/gazebo/transport/Publication.cc b/gazebo/transport/Publication.cc
index 2a5966d..e0240b1 100644
--- a/gazebo/transport/Publication.cc
+++ b/gazebo/transport/Publication.cc
@@ -15,6 +15,8 @@
*
*/
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
#include "SubscriptionTransport.hh"
#include "Publication.hh"
#include "Node.hh"
diff --git a/gazebo/transport/Publication.hh b/gazebo/transport/Publication.hh
index d51a749..de59b75 100644
--- a/gazebo/transport/Publication.hh
+++ b/gazebo/transport/Publication.hh
@@ -19,6 +19,7 @@
#define _PUBLICATION_HH_
#include <utility>
+#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <list>
diff --git a/gazebo/transport/PublicationTransport.cc b/gazebo/transport/PublicationTransport.cc
index abfb6ee..d22f62c 100644
--- a/gazebo/transport/PublicationTransport.cc
+++ b/gazebo/transport/PublicationTransport.cc
@@ -21,6 +21,8 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
#include "gazebo/transport/TopicManager.hh"
#include "gazebo/transport/ConnectionManager.hh"
#include "gazebo/transport/PublicationTransport.hh"
diff --git a/gazebo/transport/PublicationTransport.hh b/gazebo/transport/PublicationTransport.hh
index f8d5708..819ac32 100644
--- a/gazebo/transport/PublicationTransport.hh
+++ b/gazebo/transport/PublicationTransport.hh
@@ -18,6 +18,7 @@
#ifndef _PUBLICATIONTRANSPORT_HH_
#define _PUBLICATIONTRANSPORT_HH_
+#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <string>
diff --git a/gazebo/transport/Publisher.cc b/gazebo/transport/Publisher.cc
index b6f46d5..138a5c5 100644
--- a/gazebo/transport/Publisher.cc
+++ b/gazebo/transport/Publisher.cc
@@ -17,13 +17,14 @@
/* Desc: Handles pushing messages out on a named topic
* Author: Nate Koenig
*/
-
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include <ignition/math/Helpers.hh>
#include "gazebo/common/Exception.hh"
diff --git a/gazebo/transport/SubscriptionTransport.cc b/gazebo/transport/SubscriptionTransport.cc
index 0036be4..9a797a1 100644
--- a/gazebo/transport/SubscriptionTransport.cc
+++ b/gazebo/transport/SubscriptionTransport.cc
@@ -21,6 +21,8 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
#include "gazebo/transport/ConnectionManager.hh"
#include "gazebo/transport/SubscriptionTransport.hh"
diff --git a/gazebo/transport/SubscriptionTransport.hh b/gazebo/transport/SubscriptionTransport.hh
index f66cfc3..16ae677 100644
--- a/gazebo/transport/SubscriptionTransport.hh
+++ b/gazebo/transport/SubscriptionTransport.hh
@@ -17,6 +17,7 @@
#ifndef _SUBSCRIPTIONTRANSPORT_HH_
#define _SUBSCRIPTIONTRANSPORT_HH_
+#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <string>
diff --git a/gazebo/transport/TopicManager.cc b/gazebo/transport/TopicManager.cc
index 850fbe7..bb0ca21 100644
--- a/gazebo/transport/TopicManager.cc
+++ b/gazebo/transport/TopicManager.cc
@@ -24,6 +24,7 @@
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
+#include <boost/function.hpp>
#include "gazebo/msgs/msgs.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Publication.hh"
diff --git a/gazebo/transport/TopicManager.hh b/gazebo/transport/TopicManager.hh
index fe41139..3bb548c 100644
--- a/gazebo/transport/TopicManager.hh
+++ b/gazebo/transport/TopicManager.hh
@@ -18,6 +18,7 @@
#define _TOPICMANAGER_HH_
#include <boost/bind.hpp>
+#include <boost/function.hpp>
#include <map>
#include <list>
#include <string>
diff --git a/gazebo/util/Diagnostics.cc b/gazebo/util/Diagnostics.cc
index eff89db..e3968a0 100644
--- a/gazebo/util/Diagnostics.cc
+++ b/gazebo/util/Diagnostics.cc
@@ -21,6 +21,8 @@
#include <Winsock2.h>
#endif
+#include <boost/bind.hpp>
+
#include "gazebo/common/Assert.hh"
#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Events.hh"
diff --git a/gazebo/util/LogRecord.cc b/gazebo/util/LogRecord.cc
index 923cd71..8f73701 100644
--- a/gazebo/util/LogRecord.cc
+++ b/gazebo/util/LogRecord.cc
@@ -30,8 +30,10 @@
#define access _access
#endif
+#include <boost/bind.hpp>
#include <boost/date_time.hpp>
#include <boost/filesystem.hpp>
+#include <boost/function.hpp>
#include <boost/iostreams/filter/bzip2.hpp>
#include <boost/iostreams/filter/zlib.hpp>
#include <boost/iostreams/filtering_stream.hpp>
diff --git a/gazebo/util/LogRecord.hh b/gazebo/util/LogRecord.hh
index 3a4ed21..6e9650d 100644
--- a/gazebo/util/LogRecord.hh
+++ b/gazebo/util/LogRecord.hh
@@ -31,6 +31,7 @@
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/archive/iterators/ostream_iterator.hpp>
#include <boost/filesystem.hpp>
+#include <boost/function.hpp>
#include "gazebo/msgs/msgs.hh"
#include "gazebo/transport/TransportTypes.hh"
diff --git a/interfaces/player/CameraInterface.cc b/interfaces/player/CameraInterface.cc
index b37bf6a..3ed71d2 100644
--- a/interfaces/player/CameraInterface.cc
+++ b/interfaces/player/CameraInterface.cc
@@ -19,6 +19,7 @@
* Date: 2 March 2006
*/
+#include <boost/algorithm/string.hpp>
#include <math.h>
#include <iostream>
diff --git a/interfaces/player/LaserInterface.cc b/interfaces/player/LaserInterface.cc
index 259cd2a..cd75cf8 100644
--- a/interfaces/player/LaserInterface.cc
+++ b/interfaces/player/LaserInterface.cc
@@ -19,6 +19,7 @@
* Date: 2 March 2006
*/
+#include <boost/algorithm/string.hpp>
#include <math.h>
#include <iostream>
diff --git a/plugins/ModelPropShop.hh b/plugins/ModelPropShop.hh
index c166e00..22c4e97 100644
--- a/plugins/ModelPropShop.hh
+++ b/plugins/ModelPropShop.hh
@@ -70,7 +70,7 @@ namespace gazebo
private: rendering::LightPtr light;
/// \brief Pointer to the sdf document.
- private: boost::shared_ptr<sdf::SDF> sdf;
+ private: sdf::SDFPtr sdf;
/// \brief Name of the model.
private: std::string modelName;
diff --git a/plugins/MudPlugin.cc b/plugins/MudPlugin.cc
index 7a5fa4c..ede4003 100644
--- a/plugins/MudPlugin.cc
+++ b/plugins/MudPlugin.cc
@@ -15,6 +15,7 @@
*
*/
+#include <boost/algorithm/string.hpp>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
diff --git a/plugins/PressurePlugin.cc b/plugins/PressurePlugin.cc
index dc46f2f..2ce3bc2 100644
--- a/plugins/PressurePlugin.cc
+++ b/plugins/PressurePlugin.cc
@@ -18,6 +18,7 @@
* Desc: Pressure sensor plugin
* Author: Steve Peters
*/
+#include <boost/algorithm/string.hpp>
#include <gazebo/physics/Base.hh>
#include "PressurePlugin.hh"
diff --git a/plugins/events/CMakeLists.txt b/plugins/events/CMakeLists.txt
index 006d5df..328a55a 100644
--- a/plugins/events/CMakeLists.txt
+++ b/plugins/events/CMakeLists.txt
@@ -11,6 +11,7 @@ set (src
EventSource.cc
ExistenceEventSource.cc
InRegionEventSource.cc
+ JointEventSource.cc
OccupiedEventSource.cc
Region.cc
SimEventsPlugin.cc
@@ -21,6 +22,7 @@ set (inc
EventSource.hh
ExistenceEventSource.hh
InRegionEventSource.hh
+ JointEventSource.hh
OccupiedEventSource.hh
Region.hh
SimEventsException.hh
diff --git a/plugins/events/JointEventSource.cc b/plugins/events/JointEventSource.cc
new file mode 100644
index 0000000..688b999
--- /dev/null
+++ b/plugins/events/JointEventSource.cc
@@ -0,0 +1,279 @@
+/*
+ * 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 <limits>
+#include <string>
+
+#include "JointEventSource.hh"
+
+using namespace gazebo;
+
+////////////////////////////////////////////////////////////////////////////////
+JointEventSource::JointEventSource(transport::PublisherPtr _pub,
+ physics::WorldPtr _world)
+ :EventSource(_pub, "joint", _world),
+ range(INVALID),
+ isTriggered(false)
+{
+ this->min = std::numeric_limits<double>::lowest();
+ this->max = std::numeric_limits<double>::max();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void JointEventSource::Load(const sdf::ElementPtr _sdf)
+{
+ // Listen to the update event. This event is broadcast every
+ // simulation iteration.
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&JointEventSource::Update, this));
+
+ EventSource::Load(_sdf);
+
+ if (_sdf->HasElement("model"))
+ {
+ this->modelName = _sdf->Get<std::string>("model");
+ }
+ else
+ {
+ gzerr << this->name << " is missing a model element" << std::endl;
+ }
+
+ if (_sdf->HasElement("joint"))
+ {
+ this->jointName = _sdf->Get<std::string>("joint");
+ }
+ else
+ {
+ gzerr << this->name << " is missing a joint element" << std::endl;
+ }
+
+ if (_sdf->HasElement("range"))
+ {
+ sdf::ElementPtr rangeElem = _sdf->GetElement("range");
+
+ if (!rangeElem->HasElement("min") &&
+ !rangeElem->HasElement("max") )
+ {
+ gzerr << this->name << ": <range>"
+ << " should have a min and (or) a max element." << std::endl;
+ }
+
+ if (rangeElem->HasElement("min"))
+ {
+ this->min = rangeElem->Get<double>("min");
+ }
+
+ if (rangeElem->HasElement("max"))
+ {
+ this->max = rangeElem->Get<double>("max");
+ }
+
+ if (rangeElem->HasElement("type"))
+ {
+ std::string typeStr = rangeElem->Get<std::string>("type");
+ this->SetRangeFromString(typeStr);
+ if (this->range == INVALID)
+ {
+ gzerr << this->name << " has an invalid \"" << typeStr
+ << " \" range type. "
+ << " It should be one of: \"position\","
+ << " \"normalized_angle\", \"velocity\" or \"applied_force\""
+ << std::endl;
+ }
+ }
+ else
+ {
+ gzerr << this->name << ": range is missing a type element" << std::endl;
+ }
+ }
+ else
+ {
+ gzerr << this->name << " is missing a range element" << std::endl;
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void JointEventSource::Init()
+{
+ this->Info();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void JointEventSource::SetRangeFromString(const std::string &_rangeStr)
+{
+ if (_rangeStr == "position")
+ this->range = POSITION;
+ else if (_rangeStr == "normalized_angle")
+ this->range = ANGLE;
+ else if (_rangeStr == "applied_force")
+ this->range = FORCE;
+ else if (_rangeStr == "velocity")
+ this->range = VELOCITY;
+ else
+ this->range = INVALID;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+std::string JointEventSource::RangeAsString() const
+{
+ std::string rangeStr;
+ switch (this->range)
+ {
+ case POSITION:
+ rangeStr = "position";
+ break;
+ case VELOCITY:
+ rangeStr = "velocity";
+ break;
+ case ANGLE:
+ rangeStr = "normalized_angle";
+ break;
+ case FORCE:
+ rangeStr = "applied_force";
+ break;
+ default: rangeStr = "invalid";
+ break;
+ }
+ return rangeStr;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void JointEventSource::Info() const
+{
+ std::stringstream ss;
+ ss << "JointEventSource: " << this->name
+ << " model: " << this->modelName
+ << " joint: " << this->jointName
+ << " range: " << this->RangeAsString()
+ << " min: " << this->min
+ << " max: " << this->max
+ << " triggered: " << this->isTriggered
+ << std::endl;
+ gzmsg << ss.str();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+bool JointEventSource::LookupJoint()
+{
+ if (!this->model)
+ {
+ this->model = this->world->GetModel(this->modelName);
+ // if the model name is not found
+ if (!this->model)
+ {
+ // look for a model with a name that starts with our model name
+ for (unsigned int i = 0; i < this->world->GetModelCount(); ++i)
+ {
+ physics::ModelPtr m = this->world->GetModel(i);
+ size_t pos = m->GetName().find(this->modelName);
+ if (pos == 0)
+ {
+ this->model = m;
+ break;
+ }
+ }
+ }
+ }
+ // if we have a model, let's look for the joint (full joint name only)
+ if (this->model && !this->joint)
+ {
+ this->joint = this->model->GetJoint(this->jointName);
+ }
+
+ if (!this->model || !this->joint)
+ {
+ return false;
+ }
+ return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void JointEventSource::Update()
+{
+ if (!this->LookupJoint())
+ return;
+
+ bool oldState = this->isTriggered;
+ double value = 0;
+
+ double position = this->joint->GetAngle(0).Radian();
+ math::Angle a = this->joint->GetAngle(0);
+ // get a value between -PI and PI
+ a.Normalize();
+ double angle = a.Radian();
+ double force = this->joint->GetForce(0);
+ double velocity = this->joint->GetVelocity(0);
+
+ switch (this->range)
+ {
+ case POSITION:
+ {
+ value = position;
+ break;
+ }
+ case VELOCITY:
+ {
+ value = velocity;
+ break;
+ }
+ case ANGLE:
+ {
+ value = angle;
+ break;
+ }
+ case FORCE:
+ {
+ value = force;
+ break;
+ }
+ default:
+ // we can't do anything useful. Hopefully the error message during load
+ // has attracted the attention of the user.
+ return;
+ }
+
+ // check if the state has changed
+ bool currentState = value >= this->min && value <= this->max;
+ if (oldState != currentState)
+ {
+ this->isTriggered = currentState;
+ std::string json = "{";
+ if (currentState)
+ {
+ json += "\"state\":\"in_range\",";
+ }
+ else
+ {
+ json += "\"state\":\"out_of_range\",";
+ }
+ json += "\"joint\":\"" + this->jointName + "\", ";
+ json += "\"position\":\"" + std::to_string(position) + "\", ";
+ json += "\"velocity\":\"" + std::to_string(velocity) + "\", ";
+ json += "\"force\":\"" + std::to_string(force) + "\", ";
+ if (this->range == ANGLE)
+ json += "\"angle\":\"" + std::to_string(angle) + "\", ";
+
+ json += "\"range\":\"" + this->RangeAsString() + "\", ";
+ json += "\"min\":\"" + std::to_string(this->min) + "\", ";
+ json += "\"max\":\"" + std::to_string(this->max) + "\", ";
+ json += "\"value\":\"" + std::to_string(value) + "\", ";
+ json += "\"model\":\"" + this->modelName + "\"";
+ json += "}";
+ this->Emit(json);
+ }
+}
+
diff --git a/plugins/events/JointEventSource.hh b/plugins/events/JointEventSource.hh
new file mode 100644
index 0000000..567a52a
--- /dev/null
+++ b/plugins/events/JointEventSource.hh
@@ -0,0 +1,135 @@
+/*
+ * 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.
+ *
+*/
+
+#ifndef _GAZEBO_PLUGINS_EVENTS_JOINTEVENTSOURCE_HH_
+#define _GAZEBO_PLUGINS_EVENTS_JOINTEVENTSOURCE_HH_
+
+#include <string>
+
+#include "plugins/events/EventSource.hh"
+
+namespace gazebo
+{
+ /// \brief The event generator class. Events are generated when joint enters
+ /// or leaves a certain trigger state. This type of event works with joints
+ /// that have a single axis (revolute or prismatic). These are the most
+ /// common for actuated joints.
+ /// Triggers must be defined in the world, but models can be created during
+ /// the simulation. Triggers cannot overlap.
+ ///
+ ///
+ /// \verbatim
+ ///
+ /// This is an example joint event. It is triggered when the joint named
+ /// "joint" in the model "revoluter" has an angle value that enters or
+ /// leaves the range [3, 3.1416]. Triggers can also depend on the position,
+ /// velocity or applied force.
+ ///
+ /// <event>
+ /// <name>joint_angle</name>
+ /// <type>joint</type>
+ /// <model>revoluter</model>
+ /// <joint>joint</joint>
+ /// <range>
+ /// <type>normalized_angle</type>
+ /// <min>3</min>
+ /// <max>3.1416</max>
+ /// </range>
+ /// </event>
+ ///
+ /// \endverbatim
+ class JointEventSource: public EventSource
+ {
+ /// \enum Range
+ /// \brief The type of data range measured
+ public: enum Range {
+ /// \brief Absolute position (or angle, for revolute joints)
+ POSITION,
+ /// \brief Normalized angle (between -PI and PI)
+ ANGLE,
+ /// \brief Velocity or angular velocity
+ VELOCITY,
+ /// \brief Applied force (or torque, for revolute joints)
+ FORCE,
+ /// \brief invalid
+ INVALID
+ };
+
+
+ /// \brief Constructor
+ /// \param[in] _pub the publisher for the SimEvents
+ /// \param[in] _world Pointer to the world.
+ public: JointEventSource(transport::PublisherPtr _pub,
+ physics::WorldPtr _world);
+
+ /// \brief Initialize the event
+ public: virtual void Init();
+
+ /// \brief Called every simulation step
+ public: void Update();
+
+ /// \brief Prints data about the event source to the log (useful for debug)
+ public: void Info() const;
+
+ /// \brief Loads the full name of the model and the triggers from the world
+ /// file.
+ /// \param[in] _sdf The root sdf element for this joint event
+ public: virtual void Load(const sdf::ElementPtr _sdf);
+
+ /// \brief Looks for the model and the joint
+ /// \return true if found
+ private: bool LookupJoint();
+
+ /// \brief Utility range to string conversion
+ /// returns The current range as a string
+ private: std::string RangeAsString() const;
+
+ /// \brief Sets the range type from a string
+ /// \param[in] _rangeStr the range. Possible values are: "velocity",
+ /// "position", "normalized_angle" or "applied_force"
+ private: void SetRangeFromString(const std::string &_rangeStr);
+
+ /// \brief Pointer to the update event connection
+ private: event::ConnectionPtr updateConnection;
+
+ /// \brief The model used for the in trigger check.
+ private: std::string modelName;
+
+ /// \brief The joint name
+ private: std::string jointName;
+
+ /// \brief A pointer to the model
+ private: physics::ModelPtr model;
+
+ /// \brief A pointer to the Joint
+ private: physics::JointPtr joint;
+
+ /// \brief Minimum joint value (the type is determined by range member)
+ private: double min;
+
+ /// \brief Maximum joint value
+ private: double max;
+
+ /// \brief The type of data in the range (see Range enum)
+ private: Range range;
+
+ /// \brief True when the joint is currently inside the trigger condition
+ private: bool isTriggered;
+ };
+}
+
+#endif
diff --git a/plugins/events/SimEventsPlugin.cc b/plugins/events/SimEventsPlugin.cc
index 3e11409..b066d45 100644
--- a/plugins/events/SimEventsPlugin.cc
+++ b/plugins/events/SimEventsPlugin.cc
@@ -18,6 +18,7 @@
#include "InRegionEventSource.hh"
#include "ExistenceEventSource.hh"
#include "OccupiedEventSource.hh"
+#include "JointEventSource.hh"
#include "SimEventsPlugin.hh"
@@ -81,22 +82,26 @@ void SimEventsPlugin::Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
this->requestSub = this->node->Subscribe("~/request",
&SimEventsPlugin::OnRequest, this);
- // regions are defined outside of events, so that they can be shared
- // between events....
- // and we read them first
- sdf::ElementPtr child = this->sdf->GetElement("region");
- while (child)
+ // read regions, if any
+ if (this->sdf->HasElement("region"))
{
- Region *r = new Region;
- r->Load(child);
- RegionPtr region;
- region.reset(r);
- this->regions[region->name] = region;
- child = child->GetNextElement("region");
+ // regions are defined outside of events, so that they can be shared
+ // between events....
+ // and we read them first
+ sdf::ElementPtr child = this->sdf->GetElement("region");
+ while (child)
+ {
+ Region *r = new Region;
+ r->Load(child);
+ RegionPtr region;
+ region.reset(r);
+ this->regions[region->name] = region;
+ child = child->GetNextElement("region");
+ }
}
// Reading events
- child = this->sdf->GetElement("event");
+ sdf::ElementPtr child = this->sdf->GetElement("event");
while (child)
{
// get name and type of each event
@@ -125,11 +130,15 @@ void SimEventsPlugin::Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
{
event.reset(new ExistenceEventSource(this->pub, this->world) );
}
+ else if (eventType == "joint")
+ {
+ event.reset(new JointEventSource(this->pub, this->world));
+ }
else
{
std::string m;
- m = "Unknown event name: \"" + eventName;
- m += "\" of type: \"" + eventType + "\" in SimEvents plugin";
+ m = "Event \"" + eventName;
+ m += "\" is of unknown type: \"" + eventType + "\" in SimEvents plugin";
throw SimEventsException(m.c_str());
}
diff --git a/plugins/rest_web/RestApi.cc b/plugins/rest_web/RestApi.cc
index 128541b..0691c8e 100644
--- a/plugins/rest_web/RestApi.cc
+++ b/plugins/rest_web/RestApi.cc
@@ -360,8 +360,8 @@ std::string RestApi::Request(const std::string &_reqUrl,
curl_easy_cleanup(curl);
if (res != CURLE_OK)
{
- gzerr << "Request to " << url << " failed: ";
- gzerr << curl_easy_strerror(res) << std::endl;
+ gzerr << "Request to " << url << " failed: "
+ << curl_easy_strerror(res) << std::endl;
throw RestException(curl_easy_strerror(res));
}
// copy the data into a string
diff --git a/plugins/rest_web/RestWebPlugin.cc b/plugins/rest_web/RestWebPlugin.cc
index b7f994e..2ddca1c 100644
--- a/plugins/rest_web/RestWebPlugin.cc
+++ b/plugins/rest_web/RestWebPlugin.cc
@@ -20,6 +20,8 @@
#pragma comment(lib, "Rpcrt4.lib")
#else /* UNIX */
+#include <gazebo/gazebo.hh>
+
#ifdef HAVE_UUID
#include <uuid/uuid.h>
#endif
@@ -67,7 +69,11 @@ RestWebPlugin::RestWebPlugin()
#endif
#endif
-
+ if (this->session.empty())
+ {
+ // alternative to uuid
+ this->session = common::Time::GetWallTimeAsISOString();
+ }
gzmsg << "REST web Session : " << this->session << endl;
}
diff --git a/test/cmake/plugin/CMakeLists.txt b/test/cmake/plugin/CMakeLists.txt
index e62fd7a..32df313 100644
--- a/test/cmake/plugin/CMakeLists.txt
+++ b/test/cmake/plugin/CMakeLists.txt
@@ -1,9 +1,5 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-find_package(Boost REQUIRED COMPONENTS system)
-include_directories(${Boost_INCLUDE_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
-
# This GAZEBO_VERSION does not come from gazebo cmake system, it is passed
# to this test using add_definitions in the CMakeLists.txt in ../ directory
find_package(gazebo ${GAZEBO_VERSION} REQUIRED)
@@ -19,7 +15,7 @@ endif()
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(hello_world SHARED ../../testfiles/hello_world.cc)
-target_link_libraries(hello_world ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(hello_world ${GAZEBO_LIBRARIES})
diff --git a/test/examples/CMakeLists.txt b/test/examples/CMakeLists.txt
index bfd4b56..81c93d3 100644
--- a/test/examples/CMakeLists.txt
+++ b/test/examples/CMakeLists.txt
@@ -3,3 +3,8 @@ set (tests
)
add_definitions("-DCMAKE_SOURCE_DIR=\"${CMAKE_SOURCE_DIR}\"")
gz_build_tests(${tests})
+
+if (ENABLE_TESTS_COMPILATION)
+ # Increase timeout since there are many tests
+ set_tests_properties(${TEST_TYPE}_examples_build PROPERTIES TIMEOUT 500)
+endif()
diff --git a/test/examples/examples_build.cc b/test/examples/examples_build.cc
index 5574a2e..476f2f2 100644
--- a/test/examples/examples_build.cc
+++ b/test/examples/examples_build.cc
@@ -96,6 +96,7 @@ INSTANTIATE_TEST_CASE_P(Plugins, ExamplesBuild_Plugins, ::testing::Values(
, "gui_overlay_plugin_spawn"
, "gui_overlay_plugin_time"
, "hello_world"
+ , "mainwindow_example"
, "model_push"
, "model_move"
, "parameters"
@@ -113,14 +114,15 @@ TEST_P(ExamplesBuild_Standalone, Standalone)
///////////////////////////////////////////////////////////////////
INSTANTIATE_TEST_CASE_P(Standalone, ExamplesBuild_Standalone, ::testing::Values(
"actuator"
- // , "animated_box"
- // , "arrange"
+ , "animated_box"
+ , "arrange"
, "clone_simulation"
, "custom_main"
, "custom_main_pkgconfig"
- // , "listener"
- // , "publisher"
+ , "listener"
+ , "publisher"
, "test_fixture"
+ , "transporter"
));
int main(int argc, char **argv)
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 44b4e22..75467a8 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -67,7 +67,9 @@ set(tests
physics_inertia_ratio.cc
physics_link.cc
physics_msgs.cc
+ physics_msgs_inertia.cc
physics_presets.cc
+ physics_solver.cc
physics_thread_safe.cc
pioneer2dx.cc
plugin.cc
diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc
index 6c1b1b3..f7988e2 100644
--- a/test/integration/camera_sensor.cc
+++ b/test/integration/camera_sensor.cc
@@ -103,6 +103,72 @@ TEST_F(CameraSensor, CheckThrottle)
}
/////////////////////////////////////////////////
+TEST_F(CameraSensor, FillMsg)
+{
+ Load("worlds/empty_test.world");
+
+ // Make sure the render engine is available.
+ if (rendering::RenderEngine::Instance()->GetRenderPathType() ==
+ rendering::RenderEngine::NONE)
+ {
+ gzerr << "No rendering engine, unable to run camera test\n";
+ return;
+ }
+
+ // spawn sensors of various sizes to test speed
+ std::string modelName = "camera_model";
+ std::string cameraName = "camera_sensor";
+
+ // test resolution, my machine gets about 106 fps
+ unsigned int width = 320;
+ unsigned int height = 240;
+ double updateRate = 0;
+ math::Pose setPose(
+ math::Vector3(-5, 0, 5), math::Quaternion(0, GZ_DTOR(15), 0));
+ SpawnCamera(modelName, cameraName, setPose.pos,
+ setPose.rot.GetAsEuler(), width, height, updateRate);
+ sensors::SensorPtr sensor = sensors::get_sensor(cameraName);
+ sensors::CameraSensorPtr camSensor =
+ boost::dynamic_pointer_cast<sensors::CameraSensor>(sensor);
+
+ msgs::Sensor msg;
+ sensor->FillMsg(msg);
+
+ // Required fields
+ EXPECT_EQ(msg.name(), cameraName);
+ EXPECT_EQ(msg.parent(), sensor->GetParentName());
+ EXPECT_EQ(msg.type(), "camera");
+
+ // Optional fields
+ ASSERT_TRUE(msg.has_always_on());
+ EXPECT_EQ(msg.always_on(), sensor->IsActive());
+
+ ASSERT_TRUE(msg.has_pose());
+ EXPECT_EQ(msgs::ConvertIgn(msg.pose()), sensor->Pose());
+
+ ASSERT_TRUE(msg.has_topic());
+ EXPECT_EQ(msg.topic(), sensor->GetTopic());
+
+ ASSERT_TRUE(msg.has_update_rate());
+ EXPECT_EQ(msg.update_rate(), sensor->GetUpdateRate());
+
+ ASSERT_TRUE(msg.has_visualize());
+ EXPECT_EQ(msg.visualize(), sensor->GetVisualize());
+
+ ASSERT_FALSE(msg.has_contact());
+ ASSERT_FALSE(msg.has_ray());
+ ASSERT_TRUE(msg.has_camera());
+ auto cameraMsg = msg.camera();
+ auto cam = camSensor->GetCamera();
+ EXPECT_EQ(cameraMsg.horizontal_fov(), cam->GetHFOV().Radian());
+ EXPECT_EQ(cameraMsg.image_size().x(), camSensor->GetImageWidth());
+ EXPECT_EQ(cameraMsg.image_size().y(), camSensor->GetImageHeight());
+ EXPECT_EQ(cameraMsg.image_format(), cam->GetImageFormat());
+ EXPECT_EQ(cameraMsg.near_clip(), cam->GetNearClip());
+ EXPECT_EQ(cameraMsg.far_clip(), cam->GetFarClip());
+}
+
+/////////////////////////////////////////////////
TEST_F(CameraSensor, UnlimitedTest)
{
Load("worlds/empty_test.world");
diff --git a/test/integration/dem.cc b/test/integration/dem.cc
index d4126e5..dfed9cf 100644
--- a/test/integration/dem.cc
+++ b/test/integration/dem.cc
@@ -39,6 +39,11 @@ TEST_F(Dem_TEST, GPS)
// Load a DEM world with a GPS sensor (without noise) attached to a box.
Load("worlds/dem_gps.world");
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+ physics::ModelPtr model = world->GetModel("box1");
+ ASSERT_TRUE(model != NULL);
+
sensors::SensorManager *mgr = sensors::SensorManager::Instance();
// Update the sensor manager so that it can process new sensors.
@@ -61,7 +66,10 @@ TEST_F(Dem_TEST, GPS)
EXPECT_NEAR(sensor->Latitude().Degree(), latitude.Degree(), DOUBLE_TOL);
EXPECT_NEAR(sensor->Longitude().Degree(), longitude.Degree(), DOUBLE_TOL);
- EXPECT_NEAR(sensor->GetAltitude(), elevation, 1);
+
+ // Sensor altitude is the elevation of the terrain + the sensor position.
+ EXPECT_NEAR(sensor->GetAltitude(),
+ elevation + model->GetWorldPose().pos.z, 1);
}
#endif
diff --git a/test/integration/joint_gearbox.cc b/test/integration/joint_gearbox.cc
index 4818dd8..8a6cb32 100644
--- a/test/integration/joint_gearbox.cc
+++ b/test/integration/joint_gearbox.cc
@@ -41,6 +41,7 @@ void ODEGearboxJoint_TEST::GearboxTest(const std::string &_physicsEngine)
ASSERT_TRUE(world != NULL);
physics::ModelPtr model = world->GetModel("model_1");
+ physics::JointPtr joint0 = model->GetJoint("joint_02");
physics::JointPtr joint1 = model->GetJoint("joint_12");
physics::JointPtr joint3 = model->GetJoint("joint_23");
@@ -52,46 +53,61 @@ void ODEGearboxJoint_TEST::GearboxTest(const std::string &_physicsEngine)
double force3 = 1.0;
double force1 = force3 * gearboxRatio;
- int steps = 10000;
- for (int i = 0; i < steps; ++i)
- {
- joint1->SetForce(0, force1);
- joint3->SetForce(0, force3);
- world->Step(1);
- if (i%1000 == 0)
- gzdbg << "gearbox time [" << world->GetSimTime().Double()
- << "] vel [" << joint1->GetVelocity(0)
- << "] pose [" << joint1->GetAngle(0).Radian()
- << "] vel [" << joint3->GetVelocity(0)
- << "] pose [" << joint3->GetAngle(0).Radian()
- << "]\n";
- EXPECT_NEAR(joint1->GetVelocity(0), 0, 1e-6);
- EXPECT_NEAR(joint3->GetVelocity(0), 0, 1e-6);
- EXPECT_NEAR(joint1->GetAngle(0).Radian(), 0, 1e-6);
- EXPECT_NEAR(joint3->GetAngle(0).Radian(), 0, 1e-6);
- }
-
- // slight imbalance
- for (int i = 0; i < steps; ++i)
+ // repeat the same test for various joint0 angles (thanks to issue 1703)
+ int directions = 20;
+ double increments = M_PI/4.0;
+ for (int j = -directions; j < directions; j+=2)
{
- joint1->SetForce(0, -force3);
- joint3->SetForce(0, force3);
- world->Step(1);
- if (i%1000 == 0)
- gzdbg << "gearbox time [" << world->GetSimTime().Double()
- << "] vel [" << joint1->GetVelocity(0)
- << "] pose [" << joint1->GetAngle(0).Radian()
- << "] vel [" << joint3->GetVelocity(0)
- << "] pose [" << joint3->GetAngle(0).Radian()
- << "]\n";
- EXPECT_GT(joint1->GetVelocity(0), 0);
- EXPECT_GT(joint3->GetVelocity(0), 0);
- EXPECT_GT(joint1->GetAngle(0).Radian(), 0);
- EXPECT_GT(joint3->GetAngle(0).Radian(), 0);
- EXPECT_NEAR(joint1->GetVelocity(0)*gearboxRatio, -joint3->GetVelocity(0),
- TOL);
- EXPECT_NEAR(joint1->GetAngle(0).Radian()*gearboxRatio,
- -joint3->GetAngle(0).Radian(), TOL);
+ // reset world
+ world->Reset();
+ // set joint0 angle
+ double angle = static_cast<double>(j) * increments;
+ joint0->SetPosition(0, angle);
+ gzdbg << "j [" << j
+ << "] angle [" << angle
+ << "]\n";
+
+ int steps = 10000;
+ for (int i = 0; i < steps; ++i)
+ {
+ joint1->SetForce(0, force1);
+ joint3->SetForce(0, force3);
+ world->Step(1);
+ if (i%1000 == 0)
+ gzdbg << "gearbox time [" << world->GetSimTime().Double()
+ << "] vel [" << joint1->GetVelocity(0)
+ << "] pose [" << joint1->GetAngle(0).Radian()
+ << "] vel [" << joint3->GetVelocity(0)
+ << "] pose [" << joint3->GetAngle(0).Radian()
+ << "]\n";
+ EXPECT_NEAR(joint1->GetVelocity(0), 0, TOL);
+ EXPECT_NEAR(joint3->GetVelocity(0), 0, TOL);
+ EXPECT_NEAR(joint1->GetAngle(0).Radian(), 0, TOL);
+ EXPECT_NEAR(joint3->GetAngle(0).Radian(), 0, TOL);
+ }
+
+ // slight imbalance
+ for (int i = 0; i < steps; ++i)
+ {
+ joint1->SetForce(0, -force3);
+ joint3->SetForce(0, force3);
+ world->Step(1);
+ if (i%1000 == 0)
+ gzdbg << "gearbox time [" << world->GetSimTime().Double()
+ << "] vel [" << joint1->GetVelocity(0)
+ << "] pose [" << joint1->GetAngle(0).Radian()
+ << "] vel [" << joint3->GetVelocity(0)
+ << "] pose [" << joint3->GetAngle(0).Radian()
+ << "]\n";
+ EXPECT_GT(joint1->GetVelocity(0), 0);
+ EXPECT_GT(joint3->GetVelocity(0), 0);
+ EXPECT_GT(joint1->GetAngle(0).Radian(), 0);
+ EXPECT_GT(joint3->GetAngle(0).Radian(), 0);
+ EXPECT_NEAR(joint1->GetVelocity(0)*gearboxRatio, -joint3->GetVelocity(0),
+ TOL);
+ EXPECT_NEAR(joint1->GetAngle(0).Radian()*gearboxRatio,
+ -joint3->GetAngle(0).Radian(), TOL);
+ }
}
}
@@ -139,10 +155,10 @@ void ODEGearboxJoint_TEST::SetGearboxRatio(const std::string &_physicsEngine)
<< "] vel [" << joint3->GetVelocity(0)
<< "] pose [" << joint3->GetAngle(0).Radian()
<< "]\n";
- EXPECT_NEAR(joint1->GetVelocity(0), 0, 1e-6);
- EXPECT_NEAR(joint3->GetVelocity(0), 0, 1e-6);
- EXPECT_NEAR(joint1->GetAngle(0).Radian(), 0, 1e-6);
- EXPECT_NEAR(joint3->GetAngle(0).Radian(), 0, 1e-6);
+ EXPECT_NEAR(joint1->GetVelocity(0), 0, TOL);
+ EXPECT_NEAR(joint3->GetVelocity(0), 0, TOL);
+ EXPECT_NEAR(joint1->GetAngle(0).Radian(), 0, TOL);
+ EXPECT_NEAR(joint3->GetAngle(0).Radian(), 0, TOL);
}
// slight imbalance
diff --git a/test/integration/joint_screw.cc b/test/integration/joint_screw.cc
index fcb4e67..01759a3 100644
--- a/test/integration/joint_screw.cc
+++ b/test/integration/joint_screw.cc
@@ -275,12 +275,6 @@ void JointTestScrew::ScrewJointForce(const std::string &_physicsEngine)
return;
}
- if (_physicsEngine == "dart")
- {
- gzerr << "Aborting test for dart, see issues #1096.\n";
- return;
- }
-
// Load our screw joint test world
Load("worlds/screw_joint_test.world", true, _physicsEngine);
diff --git a/test/integration/physics_friction.cc b/test/integration/physics_friction.cc
index 8c722df..05aeb74 100644
--- a/test/integration/physics_friction.cc
+++ b/test/integration/physics_friction.cc
@@ -529,7 +529,17 @@ TEST_P(PhysicsFrictionTest, FrictionDemo)
/////////////////////////////////////////////////
TEST_P(WorldStepFrictionTest, FrictionDemoWorldStep)
{
- FrictionDemo("ode", "world", GetParam());
+ std::string worldStepSolver = GetParam();
+ if (worldStepSolver.compare("BULLET_PGS") == 0 ||
+ worldStepSolver.compare("BULLET_LEMKE") == 0)
+ {
+ gzerr << "Solver ["
+ << worldStepSolver
+ << "] doesn't yet work with this test."
+ << std::endl;
+ return;
+ }
+ FrictionDemo("ode", "world", worldStepSolver);
}
/////////////////////////////////////////////////
diff --git a/test/integration/physics_link.cc b/test/integration/physics_link.cc
index f9ee5cb..f4e1e26 100644
--- a/test/integration/physics_link.cc
+++ b/test/integration/physics_link.cc
@@ -15,6 +15,7 @@
*
*/
#include <string.h>
+#include <boost/algorithm/string.hpp>
#include "gazebo/math/Vector3Stats.hh"
#include "gazebo/msgs/msgs.hh"
diff --git a/test/integration/physics_msgs_inertia.cc b/test/integration/physics_msgs_inertia.cc
new file mode 100644
index 0000000..a326f59
--- /dev/null
+++ b/test/integration/physics_msgs_inertia.cc
@@ -0,0 +1,275 @@
+/*
+ * 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 <string>
+
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/physics/physics.hh"
+#include "gazebo/transport/transport.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+
+using namespace gazebo;
+
+class InertiaMsgsTest : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+ /// \brief Set inertia parameters over ~/model/modify
+ /// and verify that Inertial accessors register the change.
+ /// \param[in] _physicsEngine Type of physics engine to use.
+ public: void InertialAccessors(const std::string &_physicsEngine);
+
+ /// \brief Set center of mass of link over ~/model/modify
+ /// and verify that it causes a seesaw to unbalance.
+ /// \param[in] _physicsEngine Type of physics engine to use.
+ public: void SetCoG(const std::string &_physicsEngine);
+
+ /// \brief Set mass of link over ~/model/modify
+ /// and verify that it causes a seesaw to unbalance.
+ /// \param[in] _physicsEngine Type of physics engine to use.
+ public: void SetMass(const std::string &_physicsEngine);
+};
+
+/////////////////////////////////////////////////
+void InertiaMsgsTest::InertialAccessors(const std::string &_physicsEngine)
+{
+ Load("worlds/seesaw.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ const std::string modelName("cube1");
+ auto model = world->GetModel(modelName);
+ ASSERT_TRUE(model != NULL);
+ auto link = model->GetLink();
+ ASSERT_TRUE(link != NULL);
+ auto inertial = link->GetInertial();
+ ASSERT_TRUE(inertial != NULL);
+ const double mass = inertial->GetMass();
+ const math::Vector3 cog = inertial->GetCoG();
+ const math::Vector3 Ixxyyzz = inertial->GetPrincipalMoments();
+ const math::Vector3 Ixyxzyz = inertial->GetProductsofInertia();
+ EXPECT_DOUBLE_EQ(mass, 45.56250000000001);
+ EXPECT_EQ(cog, math::Vector3::Zero);
+ EXPECT_EQ(Ixxyyzz, 1.537734375*math::Vector3::One);
+ EXPECT_EQ(Ixyxzyz, math::Vector3::Zero);
+
+ // new inertial values
+ msgs::Model msg;
+ msg.set_name(modelName);
+ msg.add_link();
+ auto msgLink = msg.mutable_link(0);
+ msgLink->set_name("link");
+ msgLink->set_id(link->GetId());
+ auto msgInertial = msgLink->mutable_inertial();
+ msgInertial->set_mass(99.9);
+ msgInertial->set_ixx(12.3);
+ msgInertial->set_ixy(0.123);
+ msgInertial->set_ixz(0.456);
+ msgInertial->set_iyy(13.4);
+ msgInertial->set_iyz(0.789);
+ msgInertial->set_izz(15.6);
+ const ignition::math::Vector3d newCog(1.1, -2.2, 3.3);
+ msgs::Set(msgInertial->mutable_pose(), ignition::math::Pose3d(
+ newCog, ignition::math::Quaterniond()));
+
+ // Set inertial properties by publishing to "~/model/modify"
+ transport::PublisherPtr modelPub =
+ this->node->Advertise<msgs::Model>("~/model/modify");
+ modelPub->WaitForConnection();
+ modelPub->Publish(msg, true);
+
+ while (newCog != inertial->GetCoG().Ign())
+ {
+ world->Step(1);
+ common::Time::MSleep(1);
+ modelPub->Publish(msg, true);
+ }
+ EXPECT_DOUBLE_EQ(inertial->GetMass(), msgInertial->mass());
+ EXPECT_EQ(inertial->GetCoG().Ign(), newCog);
+ EXPECT_EQ(inertial->GetPrincipalMoments(),
+ ignition::math::Vector3d(
+ msgInertial->ixx(),
+ msgInertial->iyy(),
+ msgInertial->izz()));
+ EXPECT_EQ(inertial->GetProductsofInertia(),
+ ignition::math::Vector3d(
+ msgInertial->ixy(),
+ msgInertial->ixz(),
+ msgInertial->iyz()));
+}
+
+/////////////////////////////////////////////////
+TEST_P(InertiaMsgsTest, InertialAccessors)
+{
+ InertialAccessors(GetParam());
+}
+
+/////////////////////////////////////////////////
+void InertiaMsgsTest::SetCoG(const std::string &_physicsEngine)
+{
+ Load("worlds/seesaw.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // check the gravity vector
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+ math::Vector3 g = physics->GetGravity();
+ EXPECT_EQ(g, math::Vector3(0, 0, -9.8));
+
+ const std::string modelName("plank");
+ auto model = world->GetModel(modelName);
+ ASSERT_TRUE(model != NULL);
+ auto link = model->GetLink();
+ ASSERT_TRUE(link != NULL);
+ auto inertial = link->GetInertial();
+ ASSERT_TRUE(inertial != NULL);
+ const double mass = inertial->GetMass();
+ const math::Vector3 cog = inertial->GetCoG();
+ const math::Vector3 Ixxyyzz = inertial->GetPrincipalMoments();
+ const math::Vector3 Ixyxzyz = inertial->GetProductsofInertia();
+ EXPECT_DOUBLE_EQ(mass, 120);
+ EXPECT_EQ(cog, math::Vector3::Zero);
+ EXPECT_EQ(Ixxyyzz, math::Vector3(2.564, 360.064, 362.5));
+ EXPECT_EQ(Ixyxzyz, math::Vector3::Zero);
+
+ // new center of mass
+ msgs::Model msg;
+ msg.set_name(modelName);
+ msg.add_link();
+ auto msgLink = msg.mutable_link(0);
+ msgLink->set_name("link");
+ msgLink->set_id(link->GetId());
+ auto msgInertial = msgLink->mutable_inertial();
+ const ignition::math::Vector3d newCoG(2.5, 0, 0);
+ msgs::Set(msgInertial->mutable_pose(), ignition::math::Pose3d(
+ newCoG, ignition::math::Quaterniond()));
+
+ // Set inertial properties by publishing to "~/model/modify"
+ transport::PublisherPtr modelPub =
+ this->node->Advertise<msgs::Model>("~/model/modify");
+ modelPub->WaitForConnection();
+ modelPub->Publish(msg, true);
+
+ while (newCoG != inertial->GetCoG().Ign())
+ {
+ world->Step(1);
+ common::Time::MSleep(1);
+ modelPub->Publish(msg, true);
+ }
+ EXPECT_EQ(inertial->GetCoG().Ign(), newCoG);
+
+ world->Step(1000);
+ EXPECT_GT(model->GetWorldPose().rot.GetAsEuler().y, 0.25);
+}
+
+/////////////////////////////////////////////////
+TEST_P(InertiaMsgsTest, SetCoG)
+{
+ std::string physicsEngine = GetParam();
+ if (physicsEngine == "bullet" || physicsEngine == "simbody")
+ {
+ gzerr << physicsEngine
+ << " doesn't yet support dynamically changing a link's center of mass"
+ << std::endl;
+ return;
+ }
+ SetCoG(GetParam());
+}
+
+/////////////////////////////////////////////////
+void InertiaMsgsTest::SetMass(const std::string &_physicsEngine)
+{
+ Load("worlds/seesaw.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // check the gravity vector
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+ math::Vector3 g = physics->GetGravity();
+ EXPECT_EQ(g, math::Vector3(0, 0, -9.8));
+
+ const std::string modelName("cube1");
+ auto model = world->GetModel(modelName);
+ ASSERT_TRUE(model != NULL);
+ auto link = model->GetLink();
+ ASSERT_TRUE(link != NULL);
+ auto inertial = link->GetInertial();
+ ASSERT_TRUE(inertial != NULL);
+ const double mass = inertial->GetMass();
+ const math::Vector3 cog = inertial->GetCoG();
+ const math::Vector3 Ixxyyzz = inertial->GetPrincipalMoments();
+ const math::Vector3 Ixyxzyz = inertial->GetProductsofInertia();
+ EXPECT_DOUBLE_EQ(mass, 45.56250000000001);
+ EXPECT_EQ(cog, math::Vector3::Zero);
+ EXPECT_EQ(Ixxyyzz, 1.537734375*math::Vector3::One);
+ EXPECT_EQ(Ixyxzyz, math::Vector3::Zero);
+
+ // new inertial values
+ msgs::Model msg;
+ msg.set_name(modelName);
+ msg.add_link();
+ auto msgLink = msg.mutable_link(0);
+ msgLink->set_name("link");
+ msgLink->set_id(link->GetId());
+ auto msgInertial = msgLink->mutable_inertial();
+ const double newMass = 500;
+ msgInertial->set_mass(newMass);
+
+ // Set inertial properties by publishing to "~/model/modify"
+ transport::PublisherPtr modelPub =
+ this->node->Advertise<msgs::Model>("~/model/modify");
+ modelPub->WaitForConnection();
+ modelPub->Publish(msg, true);
+
+ while (!math::equal(newMass, inertial->GetMass()))
+ {
+ world->Step(1);
+ common::Time::MSleep(1);
+ modelPub->Publish(msg, true);
+ }
+ EXPECT_DOUBLE_EQ(inertial->GetMass(), msgInertial->mass());
+
+ world->Step(1000);
+ EXPECT_LT(model->GetWorldPose().pos.z, 0.40);
+}
+
+/////////////////////////////////////////////////
+TEST_P(InertiaMsgsTest, SetMass)
+{
+ std::string physicsEngine = GetParam();
+ if (physicsEngine == "simbody")
+ {
+ gzerr << physicsEngine
+ << " doesn't yet support dynamically changing a link's mass"
+ << std::endl;
+ return;
+ }
+ SetMass(physicsEngine);
+}
+
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, InertiaMsgsTest,
+ PHYSICS_ENGINE_VALUES);
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/physics_solver.cc b/test/integration/physics_solver.cc
new file mode 100644
index 0000000..f45d324
--- /dev/null
+++ b/test/integration/physics_solver.cc
@@ -0,0 +1,120 @@
+/*
+ * 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 <map>
+#include <string>
+#include <vector>
+
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/physics/physics.hh"
+#include "SimplePendulumIntegrator.hh"
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+
+#define PHYSICS_TOL 1e-2
+using namespace gazebo;
+
+class PhysicsTest : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+ public: void DropTest(const std::string &_physicsEngine,
+ const std::string &_solverType,
+ const std::string &_worldSolverType);
+};
+
+////////////////////////////////////////////////////////////////////////
+void PhysicsTest::DropTest(const std::string &_physicsEngine,
+ const std::string &_solverType,
+ const std::string &_worldSolverType)
+{
+ Load("worlds/drop_test.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ EXPECT_TRUE(world != NULL);
+
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+
+ // check the gravity vector
+ math::Vector3 gravity = physics->GetGravity();
+ EXPECT_DOUBLE_EQ(gravity.x, 0);
+ EXPECT_DOUBLE_EQ(gravity.y, 0);
+ EXPECT_DOUBLE_EQ(gravity.z, -10.0);
+
+ if (_physicsEngine == "ode")
+ {
+ // Set solver type
+ physics->SetParam("solver_type", _solverType);
+ // Set world step solver type
+ physics->SetParam("world_step_solver", _worldSolverType);
+ }
+ math::Pose pose;
+ physics::ModelPtr sphere_model = world->GetModel("sphere");
+ if (sphere_model)
+ pose = sphere_model->GetWorldPose();
+
+ double z = pose.pos.z;
+ double test_duration = 3.0;
+
+ // Dynamic duration includes the bounce back
+ double dynamic_duration = 2.4;
+ double v = 0.0;
+ double g = -10.0;
+ double dt = world->GetPhysicsEngine()->GetMaxStepSize();
+ int steps = test_duration/dt;
+ int dynamic_steps = dynamic_duration/dt;
+ for (int i = 0; i < steps; ++i)
+ {
+ v += dt * g;
+ z += dt * v;
+
+ world->Step(1);
+ physics::ModelPtr sphere_model = world->GetModel("sphere");
+ if (sphere_model)
+ {
+ math::Vector3 vel = sphere_model->GetWorldLinearVel();
+ math::Pose pose = sphere_model->GetWorldPose();
+ if (z > 0.5)
+ {
+ EXPECT_LT(fabs(vel.z - v), PHYSICS_TOL);
+ EXPECT_LT(fabs(pose.pos.z - z), PHYSICS_TOL);
+ }
+
+ // After contact with ground, and no bounce back
+ if (i > dynamic_steps)
+ {
+ EXPECT_LT(fabs(vel.z), PHYSICS_TOL);
+ EXPECT_LT(fabs(pose.pos.z - 0.5), PHYSICS_TOL);
+ }
+ }
+ }
+}
+
+TEST_P(PhysicsTest, DropTest)
+{
+ gzdbg << "DropTest(ode, world, " << GetParam() << ')' << std::endl;
+ DropTest("ode", "world", GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(WorldStepSolvers, PhysicsTest,
+ WORLD_STEP_SOLVERS);
+
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/sim_events.cc b/test/integration/sim_events.cc
index 589dc16..b0522d7 100644
--- a/test/integration/sim_events.cc
+++ b/test/integration/sim_events.cc
@@ -35,6 +35,7 @@ class SimEventsTest : public ServerFixture,
public: void SpawnAndDeleteModel(const std::string &_physicsEngine);
public: void ModelInAndOutOfRegion(const std::string &_physicsEngine);
public: void OccupiedEventSource(const std::string &_physicsEngine);
+ public: void JointEventSource(const std::string &_physicsEngine);
};
// globals to exchange data between threads
@@ -53,6 +54,7 @@ void ReceiveSimEvent(ConstSimEventPtr &_msg)
g_event_type = _msg->type();
g_event_name = _msg->name();
g_event_data = _msg->data();
+ gzdbg << "ReceiveSimEvent " << g_event_type << std::endl;
}
// get the count in a thread safe way
@@ -79,7 +81,7 @@ std::string GetEventData()
// waits for one or multiple events. if the expected number is
// specified, then the function can return early
unsigned int WaitForNewEvent(unsigned int current,
- unsigned int max_tries = 10,
+ unsigned int max_tries = 50,
unsigned int ms = 10)
{
for (unsigned int i = 0; i < max_tries; i++)
@@ -100,7 +102,7 @@ unsigned int WaitForNewEvent(unsigned int current,
////////////////////////////////////////////////////////////////////////
void SimEventsTest::SimPauseRun(const std::string &_physicsEngine)
{
- Load("test/worlds/sim_events.world", false, _physicsEngine);
+ Load("worlds/sim_events.world", false, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
// setup the callback that increments the counter everytime a
@@ -131,7 +133,7 @@ void SimEventsTest::SpawnAndDeleteModel(const std::string &_physicsEngine)
{
if (SKIP_FAILING_TESTS && _physicsEngine != "ode") return;
- Load("test/worlds/sim_events.world", false, _physicsEngine);
+ Load("worlds/sim_events.world", false, _physicsEngine);
// setup the callback that increments the counter everytime a
// SimEvent is emitted.
transport::NodePtr node = transport::NodePtr(new transport::Node());
@@ -165,7 +167,7 @@ void SimEventsTest::ModelInAndOutOfRegion(const std::string &_physicsEngine)
// simbody stepTo() failure
if (SKIP_FAILING_TESTS && _physicsEngine != "ode") return;
- Load("test/worlds/sim_events.world", false, _physicsEngine);
+ Load("worlds/sim_events.world", false, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
// setup the callback that increments the counter everytime a
// SimEvent is emitted.
@@ -227,9 +229,55 @@ void SimEventsTest::OccupiedEventSource(const std::string &_physicsEngine)
EXPECT_GT(elevatorModel->GetWorldPose().pos.z, 3.05);
}
+////////////////////////////////////////////////////////////////////////
+// JointEventSource:
+// Load test world, rotate joint and verify that events are generated
+////////////////////////////////////////////////////////////////////////
+void SimEventsTest::JointEventSource(const std::string &_physicsEngine)
+{
+ if (SKIP_FAILING_TESTS && _physicsEngine != "ode")
+ return;
+
+ this->Load("worlds/sim_events.world", false, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+
+ // Get the revoluter model
+ physics::ModelPtr model = world->GetModel("revoluter");
+ physics::JointPtr joint = model->GetJoint("joint");
+
+ // setup the callback that increments the counter everytime a
+ // SimEvent is emitted.
+ transport::NodePtr node = transport::NodePtr(new transport::Node());
+ node->Init();
+ transport::SubscriberPtr sceneSub = node->Subscribe("/gazebo/sim_events",
+ &ReceiveSimEvent);
+
+ // check that after position, we have received a new event
+ unsigned int count_before = GetEventCount();
+ // rotate joint
+
+ joint->SetPosition(0, IGN_PI);
+ // check for event
+ unsigned int count_after = WaitForNewEvent(count_before);
+ EXPECT_GT(count_after, count_before);
+
+ count_before = GetEventCount();
+
+ joint->SetVelocity(0, 3.1);
+ // check for event
+ count_after = WaitForNewEvent(count_before);
+ EXPECT_GT(count_after, count_before);
+}
+
+
// Run all test cases
INSTANTIATE_TEST_CASE_P(PhysicsEngines, SimEventsTest, PHYSICS_ENGINE_VALUES);
+TEST_P(SimEventsTest, JointEventSource)
+{
+ JointEventSource(GetParam());
+}
+
TEST_P(SimEventsTest, SimPauseRun)
{
SimPauseRun(GetParam());
diff --git a/test/integration/world_reset.cc b/test/integration/world_reset.cc
index 13923c1..238dede 100644
--- a/test/integration/world_reset.cc
+++ b/test/integration/world_reset.cc
@@ -27,14 +27,115 @@ typedef std::tr1::tuple<const char *, const char *, int> string2_int;
class WorldResetTest : public ServerFixture,
public ::testing::WithParamInterface<string2_int>
{
+ /// \brief Test to see if model pose is reset when the world is reset
+ /// \param[in] _physicsEngine Physics engine type.
+ /// \param[in] _world Name of world to load
+ /// \param[in] _resets Number of resets to perform in the test
+ public: void ModelPose(const std::string &_physicsEngine,
+ const std::string &_world, const int _resets);
+
+ /// \brief Test resetting different worlds
+ /// \param[in] _physicsEngine Physics engine type.
+ /// \param[in] _world Name of world to load
+ /// \param[in] _resets Number of resets to perform in the test
public: void WorldName(const std::string &_physicsEngine,
- const std::string &_world, int _resets);
+ const std::string &_world, const int _resets);
};
/////////////////////////////////////////////////
+void WorldResetTest::ModelPose(const std::string &_physicsEngine,
+ const std::string &_world, const int _resets)
+{
+ if (_physicsEngine == "simbody" &&
+ _world.find("pr2") != std::string::npos)
+ {
+ gzerr << "Simbody fails this test with the PR2 due to issue #1672"
+ << std::endl;
+ return;
+ }
+ if (_physicsEngine == "dart" &&
+ _world.find("pr2") != std::string::npos)
+ {
+ gzerr << "Abort test since dart does not support ray sensor in PR2, "
+ << "Please see issue #911.\n";
+ return;
+ }
+
+ Load(_world, true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+
+ double dt = physics->GetMaxStepSize();
+ unsigned int steps = 250;
+
+ // Step forward, verify time increasing
+ world->Step(steps);
+ double simTime = world->GetSimTime().Double();
+ EXPECT_NEAR(simTime, dt*steps, dt);
+
+ ignition::math::Pose3d initialPose(1, 2, 0.5, 0, 0, 1.57);
+
+ // spawn a box with known initial pose
+ math::Vector3 size(1, 1, 1);
+ SpawnBox("box", size, initialPose.Pos(), initialPose.Rot().Euler(), false);
+ physics::ModelPtr model = world->GetModel("box");
+ ASSERT_TRUE(model != NULL);
+
+ // verify box pose
+ EXPECT_EQ(model->GetWorldPose(), initialPose);
+
+ // move box to new pose
+ ignition::math::Pose3d newPose(4, 5, 0.5, 0, 0, 0);
+ model->SetWorldPose(newPose);
+ EXPECT_EQ(model->GetWorldPose(), newPose);
+
+ // Reset world repeatedly
+ for (int i = 0; i < _resets; ++i)
+ {
+ // Reset world, verify time == 0
+ world->Reset();
+ simTime = world->GetSimTime().Double();
+ EXPECT_NEAR(simTime, 0.0, dt);
+
+ // Step forward, verify time increasing
+ world->Step(steps);
+ simTime = world->GetSimTime().Double();
+ EXPECT_NEAR(simTime, dt*steps, dt);
+ }
+
+ // verify box has moved back to initial pose
+ EXPECT_EQ(model->GetWorldPose(), initialPose);
+}
+
+/////////////////////////////////////////////////
+TEST_P(WorldResetTest, ModelPose)
+{
+ std::string physics = std::tr1::get<0>(GetParam());
+
+ std::string worldName = std::tr1::get<1>(GetParam());
+ int resets = std::tr1::get<2>(GetParam());
+ gzdbg << "Physics engine [" << physics << "] "
+ << "world name [" << worldName << "] "
+ << "reset count [" << resets << "]"
+ << std::endl;
+ ModelPose(physics, worldName, resets);
+}
+
+/////////////////////////////////////////////////
void WorldResetTest::WorldName(const std::string &_physicsEngine,
- const std::string &_world, int _resets)
+ const std::string &_world, const int _resets)
{
+ if (_physicsEngine == "simbody" &&
+ _world.find("pr2") != std::string::npos)
+ {
+ gzerr << "Simbody fails this test with the PR2 due to issue #1672"
+ << std::endl;
+ return;
+ }
if (_physicsEngine == "dart")
{
gzerr << "Abort test since dart does not support ray sensor in PR2, "
diff --git a/test/pkgconfig/plugin/CMakeLists.txt b/test/pkgconfig/plugin/CMakeLists.txt
index 4c47da0..784cd10 100644
--- a/test/pkgconfig/plugin/CMakeLists.txt
+++ b/test/pkgconfig/plugin/CMakeLists.txt
@@ -1,10 +1,5 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-find_package(Boost REQUIRED COMPONENTS system)
-find_package(ignition-math2 QUIET)
-include_directories(${Boost_INCLUDE_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
-
include (FindPkgConfig)
if (PKG_CONFIG_FOUND)
pkg_check_modules(GAZEBO gazebo)
@@ -13,10 +8,10 @@ endif()
if (NOT "${GAZEBO_VERSION}" STREQUAL "${GAZEBO_EXPECTED_VERSION}")
message(FATAL_ERROR "Expected version is '${GAZEBO_EXPECTED_VERSION}' but found '${GAZEBO_VERSION}' in '${GAZEBO_PREFIX}'")
endif()
-include_directories(${GAZEBO_INCLUDE_DIRS} ${IGNITION-MATH_INCLUDE_DIRS})
-link_directories(${GAZEBO_LIBRARY_DIRS} ${IGNITION-MATH_LIBRARY_DIRS})
+include_directories(${GAZEBO_INCLUDE_DIRS})
+link_directories(${GAZEBO_LIBRARY_DIRS})
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CFLAGS}")
+list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CFLAGS}")
add_library(hello_world SHARED ../../testfiles/hello_world.cc)
-target_link_libraries(hello_world ${GAZEBO_LIBRARIES} ${IGNITION-MATH_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(hello_world ${GAZEBO_LIBRARIES})
diff --git a/test/regression/1694_world_accel.cc b/test/regression/1694_world_accel.cc
new file mode 100644
index 0000000..cc00d43
--- /dev/null
+++ b/test/regression/1694_world_accel.cc
@@ -0,0 +1,78 @@
+/*
+ * 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/rendering/rendering.hh>
+#include "gazebo/test/ServerFixture.hh"
+
+using namespace gazebo;
+
+const double g_tolerance = 1e-4;
+class Issue1694Test : public ServerFixture
+{
+ /// \brief Check acceleration value.
+ /// \param[in] _accel Acceleration to check.
+ /// \param[in] _expected Expected value.
+ public: static void CheckAccel(const math::Vector3 &_accel,
+ const math::Vector3 &_expected);
+};
+
+/////////////////////////////////////////////////
+void Issue1694Test::CheckAccel(const math::Vector3 &_accel,
+ const math::Vector3 &_expected)
+{
+ EXPECT_NEAR(_accel.x, _expected.x, g_tolerance);
+ EXPECT_NEAR(_accel.y, _expected.y, g_tolerance);
+ EXPECT_NEAR(_accel.z, _expected.z, g_tolerance);
+}
+
+/////////////////////////////////////////////////
+// \brief Test for issue #1694
+TEST_F(Issue1694Test, WorldAccel)
+{
+ Load("worlds/issue_1694.world", true);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ physics::ModelPtr model = world->GetModel("box_model");
+ ASSERT_TRUE(model != NULL);
+
+ physics::LinkPtr link = model->GetLink("box_link");
+ ASSERT_TRUE(link != NULL);
+
+ const math::Vector3 g = world->GetPhysicsEngine()->GetGravity();
+
+ world->Step(1);
+
+ // Expect box to still be falling
+ CheckAccel(link->GetRelativeLinearAccel(), g);
+ CheckAccel(link->GetWorldLinearAccel(), g);
+
+ world->Step(3000);
+
+ // The box should be resting on the ground
+ CheckAccel(link->GetRelativeLinearAccel(), math::Vector3());
+ CheckAccel(link->GetWorldLinearAccel(), math::Vector3());
+}
+
+/////////////////////////////////////////////////
+/// Main
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/regression/1702_remove_model_scene_nodes.cc b/test/regression/1702_remove_model_scene_nodes.cc
new file mode 100644
index 0000000..cc7bfce
--- /dev/null
+++ b/test/regression/1702_remove_model_scene_nodes.cc
@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2014-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 <ignition/math.hh>
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+#include "test/integration/joint_test.hh"
+#include "gazebo/gazebo_config.h"
+
+using namespace gazebo;
+
+class Issue1702Test : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+ /// \brief Test for issue #1702, spawn a model,
+ /// delete it, and respawn with the same name.
+ /// \param[in] _physicsEngine Type of physics engine to use.
+ public: void SpawnDeleteSpawnAgain(const std::string &_physicsEngine);
+};
+
+
+/////////////////////////////////////////////////
+void Issue1702Test::SpawnDeleteSpawnAgain(const std::string &_physicsEngine)
+{
+ if (_physicsEngine == "dart")
+ {
+ gzerr << "DART fails, see issue #1723.\n";
+ return;
+ }
+
+ // Load an empty world
+ Load("worlds/camera.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);
+
+ // spawn a model
+ msgs::Model model;
+ model.set_name("a_fancy_box");
+ const ignition::math::Vector3d pos(5.0, 0.0, 0.5);
+ msgs::Set(model.mutable_pose(),
+ ignition::math::Pose3<double>(pos,
+ ignition::math::Quaternion<double>::Identity));
+ const double mass = 1.5;
+ const ignition::math::Vector3d size(1.1, 1.2, 1.3);
+ msgs::AddBoxLink(model, mass, size);
+ auto link = model.mutable_link(0);
+ msgs::Set(link->mutable_pose(), ignition::math::Pose3d());
+
+ {
+ auto inertial = link->mutable_inertial();
+ msgs::Set(inertial->mutable_pose(), ignition::math::Pose3d());
+ }
+
+ auto collision = link->mutable_collision(0);
+ msgs::Set(collision->mutable_pose(), ignition::math::Pose3d());
+
+ auto linkVisual = link->mutable_visual(0);
+ msgs::Set(linkVisual->mutable_pose(), ignition::math::Pose3d());
+
+ physics::ModelPtr box = ServerFixture::SpawnModel(model);
+ EXPECT_TRUE(box != NULL);
+
+ std::string name = box->GetName();
+
+ // gzerr << "spawned"; getchar();
+
+ // delete that model
+ ServerFixture::RemoveModel(name);
+
+ // also test removal with below:
+ // world->RemoveModel(name);
+
+ int count = 0;
+ while (world->GetModel(name) != NULL && ++count < 1000)
+ {
+ common::Time::MSleep(1);
+ world->Step(1);
+ }
+ EXPECT_TRUE(world->GetModel(name) == NULL);
+ EXPECT_LT(count, 1000);
+
+ // gzerr << "deleted"; getchar();
+
+ // spawn the exact same model
+ // if this succeeds, we're OK.
+ physics::ModelPtr newBox = ServerFixture::SpawnModel(model);
+ count = 0;
+ while (world->GetModel(name) == NULL && ++count < 1000)
+ {
+ common::Time::MSleep(1);
+ world->Step(1);
+ }
+
+ EXPECT_TRUE(world->GetModel(name) != NULL);
+
+ // Look through a camera to ensure visuals are generated
+ rendering::ScenePtr scene = rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+ rendering::CameraPtr camera = scene->GetCamera("camera");
+ ASSERT_TRUE(camera != NULL);
+ int sleep = 0;
+ int maxSleep = 5;
+ rendering::VisualPtr visual;
+ while (!visual && sleep < maxSleep)
+ {
+ visual = scene->GetVisual("a_fancy_box::link_1");
+ common::Time::MSleep(100);
+ sleep++;
+ }
+ ASSERT_TRUE(visual != NULL);
+
+ // box should be visible to the camera.
+ EXPECT_TRUE(camera->IsVisible(visual));
+}
+
+TEST_P(Issue1702Test, SpawnDeleteSpawnAgain)
+{
+ SpawnDeleteSpawnAgain(GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, Issue1702Test, PHYSICS_ENGINE_VALUES);
+
+/////////////////////////////////////////////////
+/// Main
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/regression/CMakeLists.txt b/test/regression/CMakeLists.txt
index 4a7b927..54e6d95 100644
--- a/test/regression/CMakeLists.txt
+++ b/test/regression/CMakeLists.txt
@@ -36,6 +36,7 @@ set(tests
1208_world_plugin_init.cc
1375_world_reset.cc
1569_hydra_crash.cc
+ 1694_world_accel.cc
)
gz_build_tests(${tests})
@@ -49,6 +50,7 @@ gz_build_display_tests(${display_tests})
set(dri_tests
346_save_lights.cc
846_typo_in_camera.cc
+ 1702_remove_model_scene_nodes.cc
)
gz_build_dri_tests(${dri_tests})
diff --git a/test/worlds/gearbox.world b/test/worlds/gearbox.world
index d626652..13f27b5 100644
--- a/test/worlds/gearbox.world
+++ b/test/worlds/gearbox.world
@@ -15,9 +15,9 @@
<ode>
<solver>
<type>quick</type>
- <iters>100</iters>
+ <iters>200</iters>
<precon_iters>0</precon_iters>
- <sor>1.300000</sor>
+ <sor>1.000000</sor>
</solver>
<constraints>
<cfm>0.000000</cfm>
@@ -161,10 +161,10 @@
<parent>world</parent>
<child>link_2</child>
<axis>
- <xyz>1.000000 0.000000 0.000000</xyz>
+ <xyz>0.000000 0.000000 1.000000</xyz>
<limit>
- <upper>0</upper>
- <lower>0</lower>
+ <upper>-1000.0</upper>
+ <lower>1000.0</lower>
</limit>
<use_parent_model_frame>true</use_parent_model_frame>
</axis>
diff --git a/test/worlds/issue_1694.world b/test/worlds/issue_1694.world
new file mode 100644
index 0000000..c9592e1
--- /dev/null
+++ b/test/worlds/issue_1694.world
@@ -0,0 +1,73 @@
+<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>
+ </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="box_model">
+ <static>false</static>
+ <pose>0 0 1.0 0 0 0</pose>
+ <link name="box_link">
+ <visual name="box_visual">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>1.0 1.0 1.0</size>
+ </box>
+ </geometry>
+ </visual>
+ <collision name="box_collision">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>1.0 1.0 1.0</size>
+ </box>
+ </geometry>
+ </collision>
+ </link>
+ </model>
+ </world>
+</sdf>
diff --git a/tools/gz.cc b/tools/gz.cc
index ba5784a..5774eb5 100644
--- a/tools/gz.cc
+++ b/tools/gz.cc
@@ -469,7 +469,7 @@ bool ModelCommand::RunImpl()
return false;
}
- boost::shared_ptr<sdf::SDF> sdf(new sdf::SDF());
+ sdf::SDFPtr sdf(new sdf::SDF());
if (!sdf::init(sdf))
{
std::cerr << "Error: SDF parsing the xml failed" << std::endl;
@@ -495,7 +495,7 @@ bool ModelCommand::RunImpl()
sdfString += input;
}
- boost::shared_ptr<sdf::SDF> sdf(new sdf::SDF());
+ sdf::SDFPtr sdf(new sdf::SDF());
if (!sdf::init(sdf))
{
std::cerr << "Error: SDF parsing the xml failed" << std::endl;
@@ -548,7 +548,7 @@ bool ModelCommand::RunImpl()
}
/////////////////////////////////////////////////
-bool ModelCommand::ProcessSpawn(boost::shared_ptr<sdf::SDF> _sdf,
+bool ModelCommand::ProcessSpawn(sdf::SDFPtr _sdf,
const std::string &_name, const math::Pose &_pose, transport::NodePtr _node)
{
sdf::ElementPtr modelElem = _sdf->Root()->GetElement("model");
@@ -962,7 +962,7 @@ bool SDFCommand::RunImpl()
std::cerr << "Error initializing log file" << std::endl;
}
- boost::shared_ptr<sdf::SDF> sdf(new sdf::SDF());
+ sdf::SDFPtr sdf(new sdf::SDF());
if (this->vm.count("version"))
{
diff --git a/tools/gz.hh b/tools/gz.hh
index 4b93e67..89360bd 100644
--- a/tools/gz.hh
+++ b/tools/gz.hh
@@ -147,7 +147,7 @@ namespace gazebo
/// \param[in] _pose Pose of the model.
/// \param[in] _node Node for communication.
/// \return True if the spawn message was sent.
- private: bool ProcessSpawn(boost::shared_ptr<sdf::SDF> _sdf,
+ private: bool ProcessSpawn(sdf::SDFPtr _sdf,
const std::string &_name, const math::Pose &_pose,
transport::NodePtr _node);
};
diff --git a/tools/gz_TEST.cc b/tools/gz_TEST.cc
index 10fae60..b25aadf 100644
--- a/tools/gz_TEST.cc
+++ b/tools/gz_TEST.cc
@@ -17,7 +17,7 @@
#include <gtest/gtest.h>
#include <boost/lexical_cast.hpp>
-#include <boost/algorithm/string/trim.hpp>
+#include <boost/algorithm/string.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/filesystem.hpp>
@@ -299,7 +299,7 @@ TEST_F(gzTest, Model)
waitForMsg("gz model -w default -m my_box -f " + filename);
- boost::shared_ptr<sdf::SDF> sdf(new sdf::SDF());
+ sdf::SDFPtr sdf(new sdf::SDF());
EXPECT_TRUE(sdf::init(sdf));
EXPECT_TRUE(sdf::readFile(filename, sdf));
@@ -322,7 +322,7 @@ TEST_F(gzTest, Model)
cmd += filename + " | gz model -w default -m my_box -s";
waitForMsg(cmd);
- boost::shared_ptr<sdf::SDF> sdf(new sdf::SDF());
+ sdf::SDFPtr sdf(new sdf::SDF());
EXPECT_TRUE(sdf::init(sdf));
EXPECT_TRUE(sdf::readFile(filename, sdf));
@@ -622,7 +622,7 @@ TEST_F(gzTest, SDF)
descSums["1.2"] = "f524458ace57d6aabbbc2303da208f65af37ef53";
descSums["1.3"] = "74a3aa8d31f97328175f43d03410be55631fa0e1";
descSums["1.4"] = "057f26137669d9d7eeb5a8c6f51e4f4077d9ddcf";
- descSums["1.5"] = "522285759f420eba3b774e610822c357a0a683e2";
+ // descSums["1.5"] = "dddf642e1259439ce47b4664f853ac9f32432762";
// Test each descSum
for (std::map<std::string, std::string>::iterator iter = descSums.begin();
@@ -641,7 +641,7 @@ TEST_F(gzTest, SDF)
docSums["1.2"] = "27f9d91080ce8aa18eac27c9d899fde2d4b78785";
docSums["1.3"] = "ad80986d42eae97baf277118f52d7e8b951d8ea1";
docSums["1.4"] = "153ddd6ba6797c37c7fcddb2be5362c9969d97a1";
- docSums["1.5"] = "73b73f2735debfb86ca8361009ca32e3e0712ed4";
+ // docSums["1.5"] = "4e99e3a1e3497a0262d5253cbff12be4758e3c16";
// Test each docSum
for (std::map<std::string, std::string>::iterator iter = docSums.begin();
diff --git a/tools/gz_log.cc b/tools/gz_log.cc
index 03f8bb6..0dcdd98 100644
--- a/tools/gz_log.cc
+++ b/tools/gz_log.cc
@@ -21,7 +21,7 @@
#include <Winsock2.h>
#endif
-#include <boost/algorithm/string/regex.hpp>
+#include <boost/algorithm/string.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/posix_time/posix_time_io.hpp>
diff --git a/worlds/CMakeLists.txt b/worlds/CMakeLists.txt
index 88ed275..aa0edfd 100644
--- a/worlds/CMakeLists.txt
+++ b/worlds/CMakeLists.txt
@@ -55,6 +55,7 @@ set (files
rubble.world
shapes.world
shapes_layers.world
+ sim_events.world
simple_arm.world
simple_gripper.world
single_rotor_demo.world
diff --git a/worlds/seesaw.world b/worlds/seesaw.world
new file mode 100644
index 0000000..9e0fbb6
--- /dev/null
+++ b/worlds/seesaw.world
@@ -0,0 +1,211 @@
+<?xml version="1.0" ?>
+
+<sdf version='1.5'>
+ <world name='default'>
+ <physics type='ode'>
+ <ode>
+ <solver>
+ <iters>150</iters>
+ </solver>
+ </ode>
+ </physics>
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <model name='cube1'>
+ <pose>2.7 0 1.1050000000000002 0 0 0</pose>
+ <allow_auto_disable>0</allow_auto_disable>
+ <link name='link'>
+ <inertial>
+ <mass>45.56250000000001</mass>
+ <inertia>
+ <ixx>1.5377343750000003</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.5377343750000003</iyy>
+ <iyz>0</iyz>
+ <izz>1.5377343750000003</izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size>0.45 0.45 0.45</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <max_vel>0.1</max_vel>
+ <min_depth>0.01</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name='visual'>
+ <geometry>
+ <box>
+ <size>0.45 0.45 0.45</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Wood</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name='cube2'>
+ <pose>-2.7 0 1.1050000000000002 0 0 0</pose>
+ <allow_auto_disable>0</allow_auto_disable>
+ <link name='link'>
+ <inertial>
+ <mass>45.56250000000001</mass>
+ <inertia>
+ <ixx>1.5377343750000003</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.5377343750000003</iyy>
+ <iyz>0</iyz>
+ <izz>1.5377343750000003</izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size>0.45 0.45 0.45</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <max_vel>0.1</max_vel>
+ <min_depth>0.01</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name='visual'>
+ <geometry>
+ <box>
+ <size>0.45 0.45 0.45</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Wood</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name='fulcrum'>
+ <pose>0 0 0.4 0 0 0</pose>
+ <allow_auto_disable>0</allow_auto_disable>
+ <link name='link'>
+ <inertial>
+ <mass>20.000000000000004</mass>
+ <inertia>
+ <ixx>1.4833333333333338</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0833333333333337</iyy>
+ <iyz>0</iyz>
+ <izz>0.4333333333333334</izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size>0.1 0.5 0.8</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <max_vel>0.1</max_vel>
+ <min_depth>0.01</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name='visual'>
+ <geometry>
+ <box>
+ <size>0.1 0.5 0.8</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Wood</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name='plank'>
+ <pose>0 0 0.8400000000000001 0 0 0</pose>
+ <allow_auto_disable>0</allow_auto_disable>
+ <link name='link'>
+ <inertial>
+ <mass>120.0</mass>
+ <inertia>
+ <ixx>2.564</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>360.06399999999996</iyy>
+ <iyz>0</iyz>
+ <izz>362.5</izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size>6.0 0.5 0.08</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <max_vel>0.1</max_vel>
+ <min_depth>0.01</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name='visual'>
+ <geometry>
+ <box>
+ <size>6.0 0.5 0.08</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Wood</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <gui fullscreen='0'>
+ <camera name='user_camera'>
+ <pose>0 -7 2.41737 0 0.339643 1.5707963267948966</pose>
+ <view_controller>orbit</view_controller>
+ <projection_type>perspective</projection_type>
+ </camera>
+ </gui>
+ </world>
+</sdf>
diff --git a/worlds/seesaw.world.erb b/worlds/seesaw.world.erb
new file mode 100644
index 0000000..2bcc9ef
--- /dev/null
+++ b/worlds/seesaw.world.erb
@@ -0,0 +1,112 @@
+<?xml version="1.0" ?>
+<%
+ # See-saw world with balanced boxes
+ # SI units (length in meters)
+
+ # wood
+ density = 500
+
+ boxes = {
+ "cube1" => {},
+ "cube2" => {},
+ "fulcrum" => {},
+ "plank" => {},
+ }
+ boxes["fulcrum"][:size] = [0.1, 0.5, 0.8]
+ boxes["plank"][:size] = [6.0, 0.5, 0.08]
+ boxes["cube1"][:size] = [0.45, 0.45, 0.45]
+ boxes["cube2"][:size] = [0.45, 0.45, 0.45]
+ boxes.keys.each do |name|
+ box = boxes[name]
+ dx = box[:size][0]
+ dy = box[:size][1]
+ dz = box[:size][2]
+ volume = box[:size].reduce(:*)
+ box[:pose] = [0, 0, dz/2, 0, 0, 0]
+ box[:mass] = density * volume
+ box[:ixx] = box[:mass] / 12.0 * (dy**2 + dz**2)
+ box[:iyy] = box[:mass] / 12.0 * (dz**2 + dx**2)
+ box[:izz] = box[:mass] / 12.0 * (dx**2 + dy**2)
+ end
+ boxes["plank"][:pose][2] += boxes["fulcrum"][:size][2]
+ boxes["cube1"][:pose][2] += boxes["plank"][:pose][2] + boxes["plank"][:size][2]/2
+ boxes["cube2"][:pose][2] += boxes["plank"][:pose][2] + boxes["plank"][:size][2]/2
+ boxes["cube1"][:pose][0] += boxes["plank"][:size][0] * 0.45
+ boxes["cube2"][:pose][0] -= boxes["plank"][:size][0] * 0.45
+%>
+<sdf version='1.5'>
+ <world name='default'>
+ <physics type='ode'>
+ <ode>
+ <solver>
+ <iters>150</iters>
+ </solver>
+ </ode>
+ </physics>
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+ <include>
+ <uri>model://sun</uri>
+ </include>
+<%
+ boxes.keys.each do |name|
+ box = boxes[name]
+%>
+ <model name='<%= name %>'>
+ <pose><%= box[:pose].join(" ") %></pose>
+ <allow_auto_disable>0</allow_auto_disable>
+ <link name='link'>
+ <inertial>
+ <mass><%= box[:mass] %></mass>
+ <inertia>
+ <ixx><%= box[:ixx] %></ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy><%= box[:iyy] %></iyy>
+ <iyz>0</iyz>
+ <izz><%= box[:izz] %></izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size><%= box[:size].join(" ") %></size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <max_vel>0.1</max_vel>
+ <min_depth>0.01</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name='visual'>
+ <geometry>
+ <box>
+ <size><%= box[:size].join(" ") %></size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Wood</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+<%
+ end
+%>
+ <gui fullscreen='0'>
+ <camera name='user_camera'>
+ <pose>0 -7 2.41737 0 0.339643 <%= Math::PI / 2%></pose>
+ <view_controller>orbit</view_controller>
+ <projection_type>perspective</projection_type>
+ </camera>
+ </gui>
+ </world>
+</sdf>
diff --git a/worlds/sim_events.world b/worlds/sim_events.world
new file mode 100644
index 0000000..499f619
--- /dev/null
+++ b/worlds/sim_events.world
@@ -0,0 +1,236 @@
+<?xml version="1.0"?>
+<sdf version="1.5">
+ <world name="default">
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <plugin name="SimEvents" filename="libSimEventsPlugin.so">
+
+ <session></session>
+
+ <region>
+ <name>start</name>
+ <volume>
+ <!-- min and max are sdf::Vector3 -->
+ <!-- around the origin -->
+ <min>-3 -3 -3</min>
+ <max>3 3 3</max>
+ </volume>
+ </region>
+
+ <region>
+ <name>end</name>
+ <volume>
+ <!-- around 10,10 -->
+ <min>7 7 0</min>
+ <max>13 13 3</max>
+ </volume>
+ </region>
+
+ <event>
+ <name>NewBeer</name>
+ <type>existence</type>
+ <model>beer</model>
+ </event>
+
+ <event>
+ <name>Inactive</name>
+ <type>sim_state</type>
+ <!-- elements can be active or not,
+ and this property can be set at runtime or in the world file
+ with the active element
+ by default, they are.
+ -->
+ <active>false</active>
+ </event>
+
+ <event>
+ <name>All_spawn_and_delete</name>
+ <type>existence</type>
+ <!-- no model is specified -->
+ </event>
+
+ <!-- when the simulation state changes (pause/play) -->
+ <event>
+ <name>Sim</name>
+ <type>sim_state</type>
+ </event>
+
+ <!-- bounding box intersection event-->
+ <event>
+ <name>Can1_in_0_0</name>
+ <type>inclusion</type>
+ <model>can1</model>
+ <region>start</region>
+ </event>
+
+ <!-- bounding box inclusion event -->
+ <event>
+ <name>Can1_in_10_10</name>
+ <type>inclusion</type>
+ <model>can1</model>
+ <region>end</region>
+ </event>
+
+ <!-- Joint trigger events -->
+ <!-- position (linear or angular) -->
+ <event>
+ <name>joint_position</name>
+ <type>joint</type>
+ <model>revoluter</model>
+ <joint>joint</joint>
+ <range>
+ <type>position</type>
+ <min>5</min>
+ <max>10</max>
+ </range>
+ </event>
+
+
+ <!-- Angle: must be in range -PI to PI -->
+ <event>
+ <name>joint_angle</name>
+ <type>joint</type>
+ <model>revoluter</model>
+ <joint>joint</joint>
+ <range>
+ <type>normalized_angle</type>
+ <min>3</min>
+ <max>3.1416</max>
+ </range>
+ </event>
+
+ <!-- Velocity -->
+ <event>
+ <name>joint_velocity</name>
+ <type>joint</type>
+ <model>revoluter</model>
+ <joint>joint</joint>
+ <range>
+ <type>velocity</type>
+ <min>3</min>
+ <max>3.3</max>
+ </range>
+ </event>
+
+
+ <!-- Applied Force -->
+ <event>
+ <name>joint_force</name>
+ <type>joint</type>
+ <model>revoluter</model>
+ <joint>joint</joint>
+ <type>applied_force</type>
+ <range>
+ <type>applied_force</type>
+ <min>3</min>
+ <max>3.3</max>
+ </range>
+ </event>
+
+ <!-- Close the plugin element -->
+ </plugin>
+
+ <!--
+ spawn a few beers in the world (not coke cans, they have
+ meshes that can't be loaded with all engines yet
+ -->
+ <include>
+ <pose>0 0 0 0 0 0</pose>
+ <name>can1</name>
+ <uri>model://beer</uri>
+ </include>
+
+ <include>
+ <pose>5 0 0 0 0 0</pose>
+ <name>can2</name>
+ <uri>model://beer</uri>
+ </include>
+
+ <include>
+ <pose>10 0 0 0 0 0</pose>
+ <uri>model://beer</uri>
+ <name>can3</name>
+ </include>
+
+ <include>
+ <pose>15 0 0 0 0 0</pose>
+ <name>can4</name>
+ <uri>model://beer</uri>
+ </include>
+
+<!--
+Model with a revolute joint
+-->
+ <model name='revoluter'>
+ <pose>0.5 0.5 0.5 0 0 0</pose>
+ <link name='base'>
+ <pose>0 0 0 0 -0 0</pose>
+ <visual name='visual'>
+ <geometry>
+ <box>
+ <size>0.5 1 1</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Orange</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size>0.5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ </link>
+
+ <link name='needle'>
+ <pose>0.25 0 -0.2 0 -0 0</pose>
+ <visual name='visual'>
+ <geometry>
+ <cylinder>
+ <radius>0.05</radius>
+ <length>0.5</length>
+ </cylinder>
+ </geometry>
+ </visual>
+
+ <collision name='collision'>
+ <geometry>
+ <cylinder>
+ <radius>0.05</radius>
+ <length>0.5</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name='joint' type='revolute'>
+ <parent>needle</parent>
+ <child>base</child>
+
+ <pose>0 0 0 0 0 0</pose>
+ <axis>
+ <xyz>1 0 0</xyz>
+ </axis>
+ </joint>
+
+ <!-- this joint keeps the model from moving around -->
+ <joint name ='fix' type='fixed'>
+ <parent>world</parent>
+ <child>base</child>
+ </joint>
+
+ </model>
+
+ </world>
+</sdf>
--
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